added test_speed script
[ros_wild_thumper.git] / scripts / move_base.py
1 #!/usr/bin/env python
2 # -*- coding: iso-8859-15 -*-
3
4 import rospy
5 import struct
6 from i2c import *
7 from math import *
8 from geometry_msgs.msg import Twist
9
10 WHEEL_DIST = 0.248
11
12 class MoveBase:
13         def __init__(self):
14                 rospy.init_node('wild_thumper_move_base')
15                 rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
16                 self.set_speed(0, 0)
17                 rospy.loginfo("Init done")
18                 i2c_write_reg(0x50, 0x90, struct.pack("BB", 1, 1)) # switch direction
19                 self.run()
20         
21         def run(self):
22                 rate = rospy.Rate(20.0)
23                 while not rospy.is_shutdown():
24                         self.get_pos()
25                         #self.get_dist_forward()
26                         #self.get_dist_backward()
27                         #self.get_dist_left()
28                         #self.get_dist_right()
29                         rate.sleep()
30
31         def get_pos(self):
32                 s = i2c_read_reg(0x50, 0x10, 8)
33                 hall1, hall2, hall3, hall4 = struct.unpack(">hhhh", s)
34                 print hall1, hall2, hall3, hall4
35
36         def set_speed(self, left, right):
37                 if left > 0: left+=80
38                 elif left < 0: left-=80
39                 if right > 0: right+=80
40                 elif right < 0: right-=80
41
42                 if left > 255: left=255
43                 elif left < -255: left=-255
44                 if right > 255: right=255
45                 elif right < -255: right=-255
46
47                 i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", right, right, left, left))
48
49         def cmdVelReceived(self, msg):
50                 trans = msg.linear.x
51                 rot = msg.angular.z # rad/s
52
53                 right = rot*pi*WHEEL_DIST + trans
54                 left = trans*2-right
55                 self.set_speed(left, right)
56
57         # http://rn-wissen.de/wiki/index.php/Sensorarten#Sharp_GP2D12
58         def get_dist_ir(self, num):
59                 dev = i2c(0x52)
60                 s = struct.pack("B", num)
61                 dev.write(s)
62                 dev.close()
63
64                 sleep(2e-6)
65
66                 dev = i2c(0x52)
67                 s = dev.read(2)
68                 dev.close()
69
70                 val = struct.unpack(">H", s)[0]
71                 return 15221/(val - -276.42)/100;
72         
73         def get_dist_srf(self, num):
74                 dev = i2c(0x52)
75                 s = struct.pack("B", num)
76                 dev.write(s)
77                 dev.close()
78
79                 sleep(50e-3)
80
81                 dev = i2c(0x52)
82                 s = dev.read(2)
83                 dev.close()
84
85                 return struct.unpack(">H", s)[0]/1000.0
86
87         def get_dist_left(self):
88                 dist = self.get_dist_ir(0x1)
89
90         def get_dist_right(self):
91                  dist = self.get_dist_ir(0x3)
92
93         def get_dist_forward(self):
94                 dist = self.get_dist_srf(0x5)
95
96         def get_dist_backward(self):
97                 dist = self.get_dist_srf(0x7)
98
99
100 if __name__ == "__main__":
101         MoveBase()