root / quad2 / arduino / src / ros_lib / examples / ServoControl / ServoControl.pde @ c1426757
History | View | Annotate | Download (872 Bytes)
1 |
/* |
---|---|
2 |
* rosserial Servo Control Example |
3 |
* |
4 |
* This sketch demonstrates the control of hobby R/C servos |
5 |
* using ROS and the arduiono |
6 |
* |
7 |
* For the full tutorial write up, visit |
8 |
* www.ros.org/wiki/rosserial_arduino_demos |
9 |
* |
10 |
* For more information on the Arduino Servo Library |
11 |
* Checkout : |
12 |
* http://www.arduino.cc/en/Reference/Servo |
13 |
*/ |
14 |
|
15 |
#include <WProgram.h> |
16 |
|
17 |
#include <Servo.h> |
18 |
#include <ros.h> |
19 |
#include <std_msgs/UInt16.h> |
20 |
|
21 |
ros::NodeHandle nh; |
22 |
|
23 |
Servo servo; |
24 |
|
25 |
void servo_cb( const std_msgs::UInt16& cmd_msg){ |
26 |
servo.write(cmd_msg.data); //set servo angle, should be from 0-180 |
27 |
digitalWrite(13, HIGH-digitalRead(13)); //toggle led |
28 |
} |
29 |
|
30 |
|
31 |
ros::Subscriber<std_msgs::UInt16> sub("servo", servo_cb); |
32 |
|
33 |
void setup(){ |
34 |
pinMode(13, OUTPUT); |
35 |
|
36 |
nh.initNode(); |
37 |
nh.subscribe(sub); |
38 |
|
39 |
servo.attach(9); //attach it to pin 9 |
40 |
} |
41 |
|
42 |
void loop(){ |
43 |
nh.spinOnce(); |
44 |
delay(1); |
45 |
} |