From 135b68d61b383c5caec4bf36335a1e959437a0ed Mon Sep 17 00:00:00 2001
From: Bill Tyler <bill@tyler-labs.com>
Date: Thu, 21 Jan 2016 01:03:46 -0600
Subject: [PATCH] refactored drive to correctly adjust speed and reverse

---
 src/nomad/nomad_drive.py | 33 +++++++++++++++++++++++----------
 1 file changed, 23 insertions(+), 10 deletions(-)

diff --git a/src/nomad/nomad_drive.py b/src/nomad/nomad_drive.py
index 8c844bd..de69cdc 100755
--- a/src/nomad/nomad_drive.py
+++ b/src/nomad/nomad_drive.py
@@ -9,19 +9,25 @@
 roboclaw.Open("/dev/ttyACM0",115200)
 
 def drive_wheels(msg):
-	t = msg
-	x = t.linear.x
-	z = t.angular.z
+	x = msg.linear.x
+	z = msg.angular.z
 
-	r = (x-z)/2
-	l = (x+z)/2
+	#127 is max speed
+	m1 = int((x-z)*127)
+	m2 = int((x+z)*127)
 
-	m2 = (l * 64) + 64
-	m1 = (r * 64) + 64
+	print str(m1) + " " + str(m2)		
+	
+	if m1 > 0:
+		roboclaw.ForwardM1(0x80, m1)
+	else: 
+		roboclaw.BackwardM1(0x80, m1)
+
+	if m2 > 0:
+		roboclaw.ForwardM2(0x80, m2)
+	else: 
+		roboclaw.BackwardM2(0x80, m2)
 
-	print str(m1) + " " + str(m2)
-	roboclaw.ForwardM1(0x80, int(m1))
-	roboclaw.ForwardM2(0x80, int(m2))
 
 def get_diag_info(input_string):
 	command = input_string.input
@@ -115,6 +121,13 @@ def get_diag_info(input_string):
 	return returnmessage
 
 
+status = roboclaw.SetMainVoltages(0x80, 0x64, 0xa0)
+if status==False:
+	print "SetMainVoltages Failed"
+else:
+	print "SetMainVoltages Success"
+
+
 rospy.init_node('barb_drive')
 sub = rospy.Subscriber('cmd_vel', Twist, drive_wheels)
 service = rospy.Service('RoboclawDiagnostics', RoboclawDiagnostics, get_diag_info)