Statistics
| Branch: | Revision:

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
}