From 4ee96157a64df53a8e66aa5f3f075cc4ceb73c6e Mon Sep 17 00:00:00 2001
From: "Wayne C. Gramlich" <Wayne@Gramlich.Net>
Date: Tue, 12 May 2015 17:16:29 -0700
Subject: [PATCH] Implemented ~motors_reversed parameter.  Added
 ~left_motor_reversed and ~right_motor_reversed parameter.

---
 ros_arduino_python/config/arduino_params.yaml |  2 ++
 .../src/ros_arduino_python/arduino_driver.py  | 22 +++++++++++++++++++
 .../src/ros_arduino_python/base_controller.py |  5 ++++-
 3 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/ros_arduino_python/config/arduino_params.yaml b/ros_arduino_python/config/arduino_params.yaml
index 9acbe3d..96513da 100644
--- a/ros_arduino_python/config/arduino_params.yaml
+++ b/ros_arduino_python/config/arduino_params.yaml
@@ -22,6 +22,8 @@ base_frame: base_link
 #encoder_resolution: 8384 # from Pololu for 131:1 motors
 #gear_reduction: 1.0
 #motors_reversed: True
+#left_motor_reversed: True
+#right_motor_reversed: True
 
 # === PID parameters
 #Kp: 10
diff --git a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py
index f210d14..b9b6b2a 100755
--- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py
+++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py
@@ -49,6 +49,9 @@ class Arduino:
         self.encoder_count = 0
         self.writeTimeout = timeout
         self.interCharTimeout = timeout / 30.
+	self.motors_reversed = False
+	self.left_motor_reversed = False
+	self.right_motor_reversed = False
     
         # Keep things thread safe
         self.mutex = thread.allocate_lock()
@@ -92,6 +95,12 @@ class Arduino:
         '''
         self.port.close() 
     
+    def motors_configure(motors_reversed = False,
+      left_motor_reversed = False, right_motor_reversed = False):
+	self.motors_reversed = motors_reversed
+	self.left_motor_reversed = left_motor_reversed
+	self.right_motor_reversed = right_motor_reversed
+
     def send(self, cmd):
         ''' This command should not be used on its own: it is called by the execute commands
             below in a thread safe manner.
@@ -267,6 +276,12 @@ class Arduino:
             raise SerialException
             return None
         else:
+	    if self.motors_reversed:
+		values[0], values[1] = values[1], values[0]
+	    if self.left_motor_reversed:
+		values[0] = -values[0]
+	    if self.right_motor_reversed:
+		values[1] = -values[1]
             return values
 
     def reset_encoders(self):
@@ -277,6 +292,13 @@ class Arduino:
     def drive(self, right, left):
         ''' Speeds are given in encoder ticks per PID interval
         '''
+	if self.left_motor_reversed:
+	    left = -left
+	if self.right_motor_reversed:
+	    right = -right
+	if self.motors_reversed:
+	    left, right = right, left
+
         return self.execute_ack('m %d %d' %(right, left))
     
     def drive_m_per_s(self, right, left):
diff --git a/ros_arduino_python/src/ros_arduino_python/base_controller.py b/ros_arduino_python/src/ros_arduino_python/base_controller.py
index 813e873..7c3b29c 100755
--- a/ros_arduino_python/src/ros_arduino_python/base_controller.py
+++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py
@@ -50,7 +50,10 @@ class BaseController:
         
         self.accel_limit = rospy.get_param('~accel_limit', 0.1)
         self.motors_reversed = rospy.get_param("~motors_reversed", False)
-        
+	self.left_motor_reversed = rospy.get_param("~left_motor_reversed", False)
+	self.right_motor_reversed = rospy.get_param("~right_motor_reversed", False)
+	arduino.motors_configure(self.motors_reversed, self.left_motor_reverse, self.right_motor_reverse)
+
         # Set up PID parameters and check for missing values
         self.setup_pid(pid_params)