diff --git a/ros_arduino_msgs/msg/Analog.msg b/ros_arduino_msgs/msg/Analog.msg index 8ffeb67..f87b268 100644 --- a/ros_arduino_msgs/msg/Analog.msg +++ b/ros_arduino_msgs/msg/Analog.msg @@ -1,3 +1,3 @@ # Reading from a single analog IO pin. Header header -uint8 value +uint16 value diff --git a/ros_arduino_python/config/arduino_params.yaml b/ros_arduino_python/config/arduino_params.yaml index 9843bf3..7344182 100644 --- a/ros_arduino_python/config/arduino_params.yaml +++ b/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!): diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 4a24161..e9c65f0 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/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'] diff --git a/ros_arduino_python/src/arduino_sensors.py b/ros_arduino_python/src/arduino_sensors.py index b3c6b24..c5601ce 100755 --- a/ros_arduino_python/src/arduino_sensors.py +++ b/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) diff --git a/ros_arduino_python/src/base_controller.py b/ros_arduino_python/src/base_controller.py index 79bb81b..0c7656e 100755 --- a/ros_arduino_python/src/base_controller.py +++ b/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)