Servo positions are now specified in radians instead of degrees

ros_arduino_firmware/manifest.xml

@@ -1,7 +1,7 @@
 <package>
   <description brief="ros_arduino_firmware">
 
-     ros_arduino_firmware
+     ROS Arduino Firmware
 
   </description>
   <author>Patrick Goebel</author>

ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino

@@ -208,8 +208,6 @@ int runCommand() {
 #ifdef USE_SERVOS
   case SERVO_WRITE:
     servos[arg1].write(arg2);
-    Serial.print(arg1);
-    Serial.print(arg2);
     Serial.println("OK");
     break;
   case SERVO_READ:

ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h

@@ -1,11 +1,11 @@
 /* Define the attachment of any servos here.
-   The example shows two servos attached on pins 3 and 6.
+   The example shows two servos attached on pins 3 and 5.
 */
 
 #define N_SERVOS 2
 
 Servo servos [N_SERVOS];
-byte servoPins [N_SERVOS] = {3, 6};
+byte servoPins [N_SERVOS] = {3, 5};
 
 
 

ros_arduino_msgs/manifest.xml

@@ -1,7 +1,7 @@
 <package>
   <description brief="ros_arduino_msgs">
 
-     ros_arduino_msgs
+     ROS Arduino Messages
 
   </description>
   <author>Patrick Goebel</author>

ros_arduino_python/manifest.xml

@@ -1,7 +1,7 @@
 <package>
   <description brief="ros_arduino_python">
 
-     ros_arduino_python
+     ROS Arduino Python
 
   </description>
   <author>Patrick Goebel</author>

ros_arduino_python/src/arduino_driver.py

@@ -22,14 +22,17 @@
 """
 
 import thread
-from math import pi as PI
+from math import pi as PI, degrees, radians
 import os
 import time
 import sys
 from serial.serialutil import SerialException
 from serial import Serial
 
-class Arduino:  
+SERVO_MAX = 180
+SERVO_MIN = 0
+
+class Arduino:
     ''' Configuration Parameters
     '''    
     N_ANALOG_PORTS = 6
@@ -309,13 +312,15 @@ class Arduino:
 
     def servo_write(self, id, pos):
         ''' Usage: servo_write(id, pos)
+            Position is given in radians and converted to degrees before sending
         '''        
-        return self.execute_ack('s %d %d' %(id, pos))
+        return self.execute_ack('s %d %d' %(id, min(SERVO_MAX, max(SERVO_MIN, degrees(pos)))))
     
     def servo_read(self, id):
         ''' Usage: servo_read(id)
+            The returned position is converted from degrees to radians
         '''        
-        return self.execute('t %d' %id)
+        return radians(self.execute('t %d' %id))
 
     def ping(self, pin):
         ''' The srf05/Ping command queries an SRF05/Ping sonar sensor

ros_arduino_python/src/arduino_sensors.py

@@ -72,7 +72,7 @@ class Sensor(object):
             else:
                 self.msg.value = self.value
 
-            # At a timestamp and publish the message
+            # Add a timestamp and publish the message
             self.msg.header.stamp = rospy.Time.now()
             self.pub.publish(self.msg)
             

stack.xml

@@ -1,5 +1,5 @@
 <stack>
-  <description brief="ros_arduino_bridge">ros_arduino_bridge</description>
+  <description brief="ros_arduino_bridge">ROS Arduino Bridge</description>
   <author>Maintained by Patrick Goebel</author>
   <license>BSD</license>  
   <review status="unreviewed" notes=""/>