ServoControl with two or more servos on arduino rosserial
*Hello friends. I want to make the example of Servo Control of Arduino, initially with two Servos, but have not successful. I tried to change the code this way but does not work.*
ros::NodeHandle nh;
Servo servo; Servo servo2;
void servo_cb( const std_msgs::UInt16& cmd_msg){
servo.write(cmd_msg.data);
servo2.write(cmd_msg.data); //set servo angle, should be from 0-180
digitalWrite(13, HIGH-digitalRead(13)); //toggle led
}
ros::Subscriber<std_msgs::uint16> sub("servo", servo_cb); ros::Subscriber<std_msgs::uint16> sub("servo2", servo_cb); void setup() { pinMode(13, OUTPUT);
nh.initNode(); nh.subscribe(sub);
servo2.attach(7); servo.attach(9); }
void loop(){ nh.spinOnce(); delay(1); }
I hope someone can help me. Thanks for your time