Fixed odom rate bug; changed Analog.msg to int16; added PhidgetsVoltage and PhidgetsCurrent sensor types

ros_arduino_msgs/msg/Analog.msg

@@ -1,3 +1,3 @@
 # Reading from a single analog IO pin.
 Header header
-uint8 value
+uint16 value

ros_arduino_python/config/arduino_params.yaml

@@ -16,10 +16,11 @@ base_controller_rate: 10
 #motors_reversed: True
 
 # === PID parameters
-#Kp: 20
+#Kp: 10
 #Kd: 12
 #Ki: 0
 #Ko: 50
+#accel_limit: 1.0
 
 # === Sensor definitions.  Examples only - edit for your robot.
 #     Sensor type can be one of the follow (case sensitive!):

ros_arduino_python/nodes/arduino_node.py

@@ -94,6 +94,12 @@ class ArduinoROS():
         sensor_params = rospy.get_param("~sensors", dict({}))
         
         for name, params in sensor_params.iteritems():
+            # Set the direction to input if not specified
+            try:
+                params['direction']
+            except:
+                params['direction'] = 'input'
+                
             if params['type'] == "Ping":
                 sensor = Ping(self.controller, name, params['pin'], params['rate'])
             elif params['type'] == "GP2D12":
@@ -104,6 +110,10 @@ class ArduinoROS():
                 sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], direction=params['direction'])
             elif params['type'] == 'PololuMotorCurrent':
                 sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'])
+            elif params['type'] == 'PhidgetsVoltage':
+                sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'])
+            elif params['type'] == 'PhidgetsCurrent':
+                sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'])
                 
 #                if params['type'] == "MaxEZ1":
 #                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']

ros_arduino_python/src/arduino_sensors.py

@@ -217,6 +217,24 @@ class PololuMotorCurrent(AnalogFloatSensor):
         milliamps = self.controller.analog_read(self.pin) * 34
         return milliamps / 1000.0
     
+class PhidgetsVoltage(AnalogFloatSensor):
+    def __init__(self, *args, **kwargs):
+        super(PhidgetsVoltage, self).__init__(*args, **kwargs)
+        
+    def read_value(self):
+        # From the Phidgets documentation
+        voltage = 0.06 * (self.controller.analog_read(self.pin) - 500.)
+        return voltage
+    
+class PhidgetsCurrent(AnalogFloatSensor):
+    def __init__(self, *args, **kwargs):
+        super(PhidgetsCurrent, self).__init__(*args, **kwargs)
+        
+    def read_value(self):
+        # From the Phidgets documentation for the 20 amp DC sensor
+        current = 0.05 * (self.controller.analog_read(self.pin) - 500.)
+        return current
+    
 class MaxEZ1Sensor(SonarSensor):
     def __init__(self, *args, **kwargs):
         super(MaxEZ1Sensor, self).__init__(*args, **kwargs)

ros_arduino_python/src/base_controller.py

@@ -47,16 +47,18 @@ class BaseController:
         pid_params['Ki'] = rospy.get_param("~Ki", 0)
         pid_params['Ko'] = rospy.get_param("~Ko", 50)
         
-        self.motors_reversed = rospy.get_param("~motors_reversed", False)
         self.accel_limit = rospy.get_param('~accel_limit', 0.1)
+        self.motors_reversed = rospy.get_param("~motors_reversed", False)
         
         # Set up PID parameters and check for missing values
         self.setup_pid(pid_params)
             
         # How many encoder ticks are there per meter?
         self.ticks_per_meter = self.encoder_resolution * self.gear_reduction  / (self.wheel_diameter * pi)
-        self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
         
+        # What is the maximum acceleration we will tolerate when changing wheel speeds?
+        self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
+                
         # Track how often we get a bad encoder count (if any)
         self.bad_encoder_count = 0
                         
@@ -183,13 +185,32 @@ class BaseController:
             self.odomPub.publish(odom)
             
             if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
-                self.v_left = 0
-                self.v_right = 0
+                self.v_des_left = 0
+                self.v_des_right = 0
+                
+            if self.v_left < self.v_des_left:
+                self.v_left += self.max_accel
+                if self.v_left > self.v_des_left:
+                    self.v_left = self.v_des_left
+            else:
+                self.v_left -= self.max_accel
+                if self.v_left < self.v_des_left:
+                    self.v_left = self.v_des_left
+            
+            if self.v_right < self.v_des_right:
+                self.v_right += self.max_accel
+                if self.v_right > self.v_des_right:
+                    self.v_right = self.v_des_right
+            else:
+                self.v_right -= self.max_accel
+                if self.v_right < self.v_des_right:
+                    self.v_right = self.v_des_right
             
             # Set motor speeds in encoder ticks per PID loop
             if not self.stopped:
                 self.arduino.drive(self.v_left, self.v_right)
-
+                
+            self.t_next = now + self.t_delta
             
     def stop(self):
         self.stopped = True
@@ -214,8 +235,8 @@ class BaseController:
             left = x - th * self.wheel_track  * self.gear_reduction / 2.0
             right = x + th * self.wheel_track  * self.gear_reduction / 2.0
             
-        self.v_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
-        self.v_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)
+        self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
+        self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)