Diff: examples/ServoControl.cpp

Revision:

0:cb1dffdc7d05

diff -r 000000000000 -r cb1dffdc7d05 examples/ServoControl.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/examples/ServoControl.cpp	Sun Feb 15 10:54:04 2015 +0000
@@ -0,0 +1,46 @@
+//#define COMPILE_SERVOCONTROL_CODE_ROSSERIAL
+#ifdef  COMPILE_SERVOCONTROL_CODE_ROSSERIAL
+
+/*
+ * rosserial Servo Control Example
+ *
+ * This sketch demonstrates the control of hobby R/C servos
+ * using ROS and the arduiono
+ *
+ * For the full tutorial write up, visit
+ * www.ros.org/wiki/rosserial_arduino_demos
+ *
+ * For more information on the Arduino Servo Library
+ * Checkout :
+ * http://www.arduino.cc/en/Reference/Servo
+ */
+
+#include "mbed.h"
+#include "Servo.h"
+#include <ros.h>
+#include <std_msgs/UInt16.h>
+
+ros::NodeHandle  nh;
+
+Servo servo(p21);
+DigitalOut myled(LED1);
+
+void servo_cb( const std_msgs::UInt16& cmd_msg) {
+    servo.position(cmd_msg.data); //set servo angle, should be from 0-180
+    myled = !myled;  //toggle led
+}
+
+
+ros::Subscriber<std_msgs::UInt16> sub("servo", servo_cb);
+
+int main() {
+
+    nh.initNode();
+    nh.subscribe(sub);
+
+    while (1) {
+        nh.spinOnce();
+        wait_ms(1);
+    }
+}
+#endif
\ No newline at end of file