added test_speed script

diff --git a/data/test_speed.log b/data/test_speed.log
new file mode 100644 (file)
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 4522c5ff52e0c5b8a999f9213f5bd48c76c53c62..c597c85b150e042d33d5cd51d24a69044a30c1b3 100755 (executable)
--- 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 fbd265fb9c6001c75879da1d0cb406ab7d110f9a..5ece2683f6d0ad31cee0dd0cebfd9efdb95b26e2 100755 (executable)
--- 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 (executable)
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))