From 9614e9b29abf286b580781fcb42419d89b18f0ae Mon Sep 17 00:00:00 2001
From: Patrick Goebel <patrick@pirobot.org>
Date: Sun, 23 Dec 2012 15:13:51 -0800
Subject: [PATCH] Changed from degrees to radians when specifying servos
 positions

---
 ros_arduino_firmware/manifest.xml             |   2 +-
 .../MegaRobogaiaPololu/MegaRobogaiaPololu.ino |   2 --
 .../src/libraries/MegaRobogaiaPololu/servos.h |   4 ++--
 ros_arduino_msgs/manifest.xml                 |   2 +-
 ros_arduino_python/manifest.xml               |   2 +-
 ros_arduino_python/src/arduino_driver.py      |  13 +++++++++----
 ros_arduino_python/src/arduino_driver.pyc     | Bin 13942 -> 14252 bytes
 ros_arduino_python/src/arduino_sensors.py     |   2 +-
 ros_arduino_python/src/arduino_sensors.pyc    | Bin 12162 -> 12162 bytes
 stack.xml                                     |   2 +-
 10 files changed, 16 insertions(+), 13 deletions(-)

diff --git a/ros_arduino_firmware/manifest.xml b/ros_arduino_firmware/manifest.xml
index d601871..245bdf3 100644
--- a/ros_arduino_firmware/manifest.xml
+++ b/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>
diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino
index ccbae63..0f66429 100644
--- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino
+++ b/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:
diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h
index 4a0d92d..6100723 100644
--- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h
+++ b/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};
 
 
 
diff --git a/ros_arduino_msgs/manifest.xml b/ros_arduino_msgs/manifest.xml
index dd70516..62ebb0b 100644
--- a/ros_arduino_msgs/manifest.xml
+++ b/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>
diff --git a/ros_arduino_python/manifest.xml b/ros_arduino_python/manifest.xml
index f12f132..1d04908 100644
--- a/ros_arduino_python/manifest.xml
+++ b/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>
diff --git a/ros_arduino_python/src/arduino_driver.py b/ros_arduino_python/src/arduino_driver.py
index 1228437..c5f0e43 100755
--- a/ros_arduino_python/src/arduino_driver.py
+++ b/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
diff --git a/ros_arduino_python/src/arduino_driver.pyc b/ros_arduino_python/src/arduino_driver.pyc
index 65e6182f305d054eecafe342c7a4a4a913e7fde3..5c14558c6cc49a752d92860793f1e1a6bae0dcb4 100644
GIT binary patch
delta 2534
zcmZ`*>u(!H5TCUZC-Eb>JnW>2lU`n}^G@?3X@RzskUnVIUQi#V!MMIna;bfG?=DFz
z6@{u1RQ-S`TclD6DFPvc=$8`s28h3)LIQ*;#0Neh@c~sr0#$&@%$^&Ys&#xn&+g97
z>~CgvE`O$PWt+Tzg#4aMKORj%{!@m(d+@O@Tx;rB&6GmC6f!=D`+(^TbPuG<AYKM}
zxg6r<59JDoS4g=8(tgMUARch{ltMZPnGnQ7I7U;Ipref{(92;CK(Bx`fXe`<;tKF;
z(EV`M1J)AbWw5$f1A0J?zW_Z5@mer_prff;&?_Nc2V4QY3OEjNa~<fozXpwY<6+=o
zSd5Tirq{yMeqIiX^`O_mWrj0hn4<LtItorq?bjpj%$CATJ)EA}e|`t>M$j8b>-pJs
zh&M^YQP5Enjd_6OEjLQb=la<Ry~(xJEWMMT7WW)cbYvoB56r}xH!d;N+wR3!J066b
z;ZW>#K5ubuOW8{5M$)v!t&%Rk@i{IU_`t>wC9~c@IZ9dufnTimtc7TtNEoK!BobX4
zcgiQ3Qlvt9LDBAC3(Lup!e)2nOMeIRieLPVEF%8#-(b&*cLS?+r0bAwHpk5vBh`qw
zH?&r|l|*qnct)CYN^zJ<XKh}rqvGTVaXa*VL1)D>93CUGXy<0-b#__|RDC<-r-@Bp
z7EkJ0h^DF$q;1==j9iSoXb}Rx*r9yF&WN9s-<2LYf$-4o-Rc``Sy(l{wrwss6lBA6
zq{X}*ZR?`Qi3hbo_L>OR{oA{_L94-K*L5y!IC6g`nbQV}{EUX*XK!wN7rw(-hq%|c
zYiEE=DzuUEm`t5pGA!=$B8GG6C41q3nOkzSR?;jus}}VQ&7IPAfnysP@*$W=p4(bC
z?QswMb3?#)4TtuSXpPoyly3RWJ;I*j<XsyjkZB$xNje)LiQ=g}MKDf4LD7Z?9^+b@
zpato4SVqYot!AHXM5ARMNq0ibwscSQqSACMNUBeCUBr|0T>s*mmX@&0CGjkB)+e2<
zY_0O$#RYqW7>W<K{uOu@hcSu=md;12%G#vj!PZLlw>Z_hrVs~hCn*)`)Yph(4-+eG
zSKD@>DD#-+rc0f|QB({r&5zq7tW12}?zj%~wy;c^D^cCzHg^)-Ol5U$*$S@ORL$TO
zyL*R((mCVql97!FvvZw=H>$RU7#k4H+nf5RE*!Em&y!Zd;`&m`F|wvMjZ-mNbj$li
zk`4>L{XT0HiC88e*9%9fpet!R*<7cX9vBm^_xQwmmp>xOyO#^sCD)u`iet(pg-SDN
zCezt@^qSPW#j)-qVHr{PsN^tCKm~S1Jkt|oed1ux3(Eg(YH?h?&IWP&b7G}8^h6VD
zqPee{4U=ATO6}C~>!#&VpW>~)_t+Tf)$XHSwhtehGI<eT5qlwniwqq6dWa9UBD|9q
zJ=ABh@8WmP0}&j|$TZr^Vy-{7s^aHp);1`zs$r}1#xh62RP)_bO`5uz%9_jEa=5NK
zSvB8s)iZo9YjM@)rf!(?elkQKn=)hPS;z_`7Fs8QLx-N3)Q%rX9Nv3UhP*kGJYPJ0
zU|OTh#qu@rY5xbiCpPP*Bt`k6hf*rp*Q4OL0l?R-3;<Cqu$&Wb478o4gT1uCRf{`I
zmWe!aMU)Hd=B#XHlOIye$DqKC{5q5#SvV3N$+<2s+M&V9!0y7ih1u0B-X6UFxB|iC
z50$e?v2AF4mHMbwM?k!^76SS5P)TYhko}>bq(K7NsptqA-B?YAdXl6@f&&DH2<T2|
zCkajwyi9PKV3yz&f>)su5&y*iZJuC3tdCytY1Cue#f9O$USEYzd^J2?{4RZG<d8Tz
za&=W6@+|E_8}3UmlXMm|9DsI=K;B0guqGNd5Il|GP+zgRWhB%6?$45Aq-D#I{oI|?
zDDYa6AVnY>zk!l`-<o1_v{#YQ+Cr<6-K@AYS{IPaBxM!p+>DY-pC`zoEGYjJrg(hJ
N=c#=F<3bzj_zx(R+=&1H

delta 2101
zcmah}O>9(E6h3#_`RTm*c{81vmQHDh{=gJkpwJGWEKrbAF`!oiGFs{|?^32r-wgBK
zL}E-DLl;PlhMbK=vBs4~5@Tk=)}0IgR<2Ccl?xMHm=M4iJm<dJsRZM6`pvoLp7Wi*
zd+)8<oocq@k7)Qle)Vh_);>Y{Jw*Rb>3Vi#Sy%!rA5{HN^uwAQfMVbuIS9p|mOG&u
zf~77fc7gWsLp2Ob1{4ii2jDz_6@(RlD*$JT5l~UELU7RsjX9zGuowfYOD}JN6^3FQ
zYzkq(GD%hhiYBNCSW!^4jDa%2BEclGj$#5-0v3}@EUW~~98&>U>;{W8f;8PQ!}}?)
zQmmLcW~Dt%x~<8O`tl^G3|KvEbYyNAiapGGWU&`4`VyHB#2WJ^&3q{%PFsB*7ilOu
z$LaI($P%MXCHvs{e1GeP$aNTkdg}TJ>J|K<bF3Bj&vh69T1)P@3}Iqy1^*01#MV}*
z>nUMw*y8;#&V^UfTFE49PTtO)@Q4tYHTpyf3&y+Ras1X;PO+`qNl>dRJI~=|ih^7T
z$jv05#9t%lG@t9IWw}~&)CQxV93brrfU9F!490HfG#gvK+NiiHuX9Mz((PdHB@1^C
zzJagAL*fip;@|BFkyfr}&`>2y8mx-53)47mqf*baB$<Zlzi56U<}sc4Ejy+a1P_HN
zl`rZrtI3FP@K*Adp><20M#Xk@fVP|5rrWxj?CPY=*YIfSFJs+Dns_3eF(%r9u$I0l
zR$9@_H$n{I*?|KGPvmvlF|u)aPNnjtW~HIL(!!)TUu`-UX6<^@m4mdDZT2`m*SmRC
zv$xC4k&VlJv8jIL$UOV#BqL0(E~|3WRd2V3HwAoN3Uep&5jNr!<X#4Lms1Qn;vSZ`
zuX389z@VqSo25s@m-~1_N1oK}IntjHpS4c)2fdKx1KY{q|H5u4l<B<Kcz<9Zt>>9z
zWjoLg4rFpK*GinHVSeBGVz65n2T7aPMWyh^Tu$WJHqOm$)~%AO=Ia&fF5;FIlZ%o2
z*}Q$6gR++t4*)+Ox;AuxM12&h%J!PTjoEM>yn=ro&WKLDJM4O~*U9JtjwD4CW25hQ
zJAAn5)_gqrp-8r7w?@S*r{E?DlwGb_sxjYCPOaK>D>Ym4m|dRJf-3Qb)huiMGo;Gv
zTdrI4G%e+ET-|n0^x%#BlA-t4lX#2f{=wK$L)YouGyHw*J+TdI<4@w(+oNgSFfTQx
zdXYh&p#-0ll3l9S7UuOm-Ht0ej+z^ow7nks*>O`$5ar4Lro+<{3AD!#<EIl*^WmWQ
z<-`rKmk18}oKd{JGr!s45}n+rMPF|~8+`EfT_1@86;|*$gE+T)Xwc!e$z8X7D9L%9
zq&*p<*NmL7HF@t*bzJq=EB3-6oZY*B+2rDQG&0T5&%jTee#+z)mh_<=XK5#czB(LO
z^1+dut~|`p%P_+*%fJJYrx;E%oM9+3yu|P_!&#ug%My#{7|Ix%dehHC%HjJ{PjvXZ
z{P@$<{(rsr!_&w86|w-dr>{+9+3h?7Pf@?zOC|S$qy@;644W7-43FWiLSL4TjDAcU
z)u@!JQXdw5E-bVQ)4OyBde~lP%*pDvw^6bel*`@~FL_<n0A+j?i~Ewj+>wAUB1{p{
Qf2J^f-4CFESNDzl4Xzh>2mk;8

diff --git a/ros_arduino_python/src/arduino_sensors.py b/ros_arduino_python/src/arduino_sensors.py
index ced0fa0..b3c6b24 100755
--- a/ros_arduino_python/src/arduino_sensors.py
+++ b/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)
             
diff --git a/ros_arduino_python/src/arduino_sensors.pyc b/ros_arduino_python/src/arduino_sensors.pyc
index 5cdbfe2b3b78422fec35fd68623f1a334c596712..079bd6d3f95d2d7f40e45ba11ac113b385bdf930 100644
GIT binary patch
delta 16
XcmZpQZ;EGU{>;nuKK<H8_G*0qHHQXe

delta 16
XcmZpQZ;EGU{>;lIZg6%Zd$m3QE+_>}

diff --git a/stack.xml b/stack.xml
index e31df54..f4c5a70 100644
--- a/stack.xml
+++ b/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=""/>