From 6bbe85b249210dd265f51cfde4288530d9bb05fb Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Fri, 14 Aug 2015 02:33:47 +0000 Subject: [PATCH] added test_speed script --- data/test_speed.log | 100 ++++++++++++++++++++++++++++++++++++++++++ scripts/i2c.py | 4 +- scripts/move_base.py | 13 +++--- scripts/test_speed.py | 25 +++++++++++ 4 files changed, 135 insertions(+), 7 deletions(-) create mode 100644 data/test_speed.log create mode 100755 scripts/test_speed.py diff --git a/data/test_speed.log b/data/test_speed.log new file mode 100644 index 0000000..2df2737 --- /dev/null +++ b/data/test_speed.log @@ -0,0 +1,100 @@ +0.000: 1 -1 0 0 +0.012: 19 16 14 11 +0.024: 45 43 37 32 +0.036: 81 79 65 57 +0.048: 115 125 103 82 +0.060: 148 182 151 108 +0.072: 177 246 207 130 +0.084: 203 316 270 151 +0.096: 227 395 341 170 +0.108: 249 478 418 190 +0.121: 276 569 495 212 +0.137: 315 674 587 244 +0.151: 355 759 663 272 +0.165: 404 838 740 320 +0.179: 459 914 809 369 +0.192: 513 983 873 413 +0.205: 567 1053 943 462 +0.217: 623 1129 1013 513 +0.230: 678 1209 1086 561 +0.243: 737 1291 1159 611 +0.256: 797 1369 1230 666 +0.269: 862 1445 1301 725 +0.282: 931 1521 1370 785 +0.294: 1000 1595 1440 849 +0.307: 1069 1669 1511 915 +0.321: 1143 1750 1589 985 +0.333: 1215 1826 1667 1054 +0.347: 1288 1902 1744 1127 +0.360: 1375 1989 1831 1212 +0.375: 1453 2068 1909 1290 +0.388: 1529 2142 1985 1364 +0.400: 1608 2222 2064 1443 +0.413: 1688 2305 2145 1521 +0.426: 1769 2387 2225 1598 +0.438: 1851 2470 2307 1680 +0.451: 1934 2556 2391 1764 +0.464: 2023 2643 2477 1848 +0.477: 2111 2732 2565 1934 +0.490: 2199 2818 2655 2022 +0.503: 2300 2917 2754 2119 +0.517: 2390 3007 2843 2204 +0.530: 2480 3099 2931 2292 +0.543: 2569 3190 3020 2378 +0.556: 2659 3281 3108 2465 +0.571: 2768 3389 3217 2576 +0.584: 2859 3481 3307 2668 +0.597: 2956 3580 3411 2775 +0.611: 3061 3687 3506 2873 +0.625: 3156 3784 3601 2969 +0.638: 3255 3883 3698 3069 +0.652: 3359 3989 3802 3173 +0.665: 3455 4087 3898 3269 +0.677: 3554 4187 3998 3368 +0.691: 3655 4287 4098 3466 +0.703: 3753 4384 4196 3563 +0.716: 3854 4484 4297 3663 +0.730: 3958 4586 4398 3764 +0.742: 4058 4686 4496 3861 +0.755: 4156 4785 4594 3960 +0.767: 4255 4884 4692 4056 +0.780: 4354 4984 4790 4155 +0.793: 4454 5085 4888 4255 +0.805: 4554 5186 4987 4358 +0.818: 4656 5290 5088 4462 +0.831: 4757 5393 5190 4567 +0.844: 4861 5498 5292 4671 +0.857: 4962 5602 5394 4773 +0.870: 5065 5706 5496 4875 +0.882: 5168 5809 5599 4977 +0.895: 5271 5912 5703 5078 +0.908: 5376 6016 5807 5182 +0.920: 5480 6119 5910 5286 +0.933: 5584 6222 6014 5388 +0.946: 5689 6327 6117 5491 +0.958: 5795 6433 6223 5597 +0.971: 5900 6538 6326 5700 +0.984: 6008 6646 6434 5808 +0.997: 6115 6754 6538 5914 +1.010: 6217 6858 6640 6020 +1.023: 6320 6964 6743 6126 +1.036: 6427 7071 6848 6234 +1.048: 6530 7176 6952 6338 +1.061: 6634 7281 7057 6442 +1.073: 6739 7386 7161 6547 +1.092: 6893 7542 7315 6699 +1.105: 7001 7650 7423 6808 +1.118: 7110 7759 7534 6917 +1.131: 7218 7865 7639 7021 +1.143: 7324 7969 7744 7125 +1.156: 7431 8077 7851 7232 +1.169: 7537 8184 7956 7337 +1.181: 7642 8289 8059 7442 +1.194: 7746 8395 8162 7547 +1.207: 7850 8501 8267 7654 +1.219: 7956 8609 8372 7763 +1.232: 8063 8717 8478 7872 +1.245: 8176 8831 8591 7986 +1.258: 8282 8939 8697 8092 +1.271: 8390 9050 8806 8203 +1.286: 8516 9176 8932 8328 diff --git a/scripts/i2c.py b/scripts/i2c.py index 4522c5f..c597c85 100755 --- a/scripts/i2c.py +++ b/scripts/i2c.py @@ -66,14 +66,14 @@ class i2c: def i2c_write_reg(addr, reg, buf): dev = i2c(addr) - s = struct.pack("B", reg) + buf + s = struct.pack(">B", reg) + buf dev.write(s) dev.close() def i2c_read_reg(addr, reg, num=1): dev = i2c(addr) - s = struct.pack("B", reg) + s = struct.pack(">B", reg) dev.write(s) s = dev.read(num) dev.close() diff --git a/scripts/move_base.py b/scripts/move_base.py index fbd265f..5ece268 100755 --- a/scripts/move_base.py +++ b/scripts/move_base.py @@ -19,14 +19,20 @@ class MoveBase: self.run() def run(self): - rate = rospy.Rate(10.0) + rate = rospy.Rate(20.0) while not rospy.is_shutdown(): + self.get_pos() #self.get_dist_forward() #self.get_dist_backward() #self.get_dist_left() #self.get_dist_right() rate.sleep() + def get_pos(self): + s = i2c_read_reg(0x50, 0x10, 8) + hall1, hall2, hall3, hall4 = struct.unpack(">hhhh", s) + print hall1, hall2, hall3, hall4 + def set_speed(self, left, right): if left > 0: left+=80 elif left < 0: left-=80 @@ -38,10 +44,7 @@ class MoveBase: if right > 255: right=255 elif right < -255: right=-255 - dev = i2c(0x50) - s = struct.pack(">Bhhhh", 0x1, right, right, left, left) - dev.write(s) - dev.close() + i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", right, right, left, left)) def cmdVelReceived(self, msg): trans = msg.linear.x diff --git a/scripts/test_speed.py b/scripts/test_speed.py new file mode 100755 index 0000000..022112e --- /dev/null +++ b/scripts/test_speed.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python +# -*- coding: iso-8859-15 -*- + +import struct +from i2c import * +from datetime import datetime +from time import sleep + +def get_pos(): + s = i2c_read_reg(0x50, 0x10, 8) + hall1, hall2, hall3, hall4 = struct.unpack(">hhhh", s) + return hall1, hall2, hall3, hall4 + +if __name__ == "__main__": + i2c_write_reg(0x50, 0x90, struct.pack("BB", 1, 1)) # switch direction + speed = 255 + i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", speed, speed, speed, speed)) + start = datetime.now() + for i in range(100): + diff = datetime.now() - start + status = get_pos() + print "%d.%03d: %d %d %d %d" % ((diff.seconds, diff.microseconds/1000) + status) + sleep(0.01) + speed = 0 + i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", speed, speed, speed, speed)) -- 2.39.2