root / quad2 / arduino / src / ros_lib / examples / ServoControl / ServoControl.pde @ c1426757
History | View | Annotate | Download (872 Bytes)
1 | c1426757 | Tom Mullins | /* |
---|---|---|---|
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 | } |