Implemented ~motors_reversed parameter. Added ~left_motor_reversed and ~right_motor_reversed parameter.

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

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):

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)