ServoControl.cpp
Go to the documentation of this file.
00001 /*
00002  * rosserial Servo Control Example
00003  *
00004  * This sketch demonstrates the control of hobby R/C servos
00005  * using ROS and the arduiono
00006  *
00007  * For the full tutorial write up, visit
00008  * www.ros.org/wiki/rosserial_mbed_demos
00009  *
00010  */
00011 
00012 #include "mbed.h"
00013 #include "Servo.h"
00014 #include <ros.h>
00015 #include <std_msgs/UInt16.h>
00016 
00017 ros::NodeHandle  nh;
00018 
00019 #ifdef TARGET_LPC1768
00020 Servo servo(p21);
00021 #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE)
00022 Servo servo(D8);
00023 #else
00024 #error "You need to specify a pin for the Servo"
00025 #endif
00026 DigitalOut myled(LED1);
00027 
00028 void servo_cb( const std_msgs::UInt16& cmd_msg) {
00029     servo.position(cmd_msg.data); //set servo angle, should be from 0-180
00030     myled = !myled;  //toggle led
00031 }
00032 
00033 
00034 ros::Subscriber<std_msgs::UInt16> sub("servo", servo_cb);
00035 
00036 int main() {
00037 
00038     nh.initNode();
00039     nh.subscribe(sub);
00040 
00041     while (1) {
00042         nh.spinOnce();
00043         wait_ms(1);
00044     }
00045 }
00046 


rosserial_mbed
Author(s): Gary Servin
autogenerated on Sat Oct 7 2017 03:08:46