X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fmove_base.py;fp=scripts%2Fmove_base.py;h=1188e2c8cd0b8abda78dab2ce154a1fd05c26b05;hp=0000000000000000000000000000000000000000;hb=3d875e0b7d29c59be6a65b13cd4e2b0d157b6744;hpb=dcdf9baa260c03d34c55339af9950155319c41f0

diff --git a/scripts/move_base.py b/scripts/move_base.py
new file mode 100755
index 0000000..1188e2c
--- /dev/null
+++ b/scripts/move_base.py
@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+import rospy
+import struct
+from i2c import i2c
+from math import *
+from geometry_msgs.msg import Twist
+
+WHEEL_DIST = 0.248
+
+class MoveBase:
+	def __init__(self):
+		rospy.init_node('wild_thumper_move_base')
+		rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
+		self.set_speed(0, 0)
+		rospy.loginfo("Init done")
+		self.run()
+	
+	def run(self):
+		rate = rospy.Rate(10.0)
+		while not rospy.is_shutdown():
+			rate.sleep()
+
+	def set_speed(self, left, right):
+		if left > 255: left=255
+		elif left < -255: left=-255
+		if right > 255: right=255
+		elif right < -255: right=-255
+
+		dev = i2c(0x56)
+		s = struct.pack(">Bhh", 0x1, left, right)
+		dev.write(s)
+		dev.close()
+
+	def cmdVelReceived(self, msg):
+		trans = msg.linear.x
+		rot = msg.angular.z # rad/s
+
+		right = rot*pi*WHEEL_DIST + trans
+		left = trans*2-right
+		self.set_speed(left, right)
+
+
+if __name__ == "__main__":
+	MoveBase()