Servo control arduino [closed]

edit

I want to control a servo attached to arduino by subscribing to /joint_states topic so that when the change the slider position the servo moves accordingly. I tried with the following code but the servo doesn't change its position when i change the slider position on rviz. The led on Arduino blinks as written in the callback function but the servo isn't moving.

#if defined(ARDUINO) && ARDUINO >= 100
  #include "Arduino.h"
#else
  #include <WProgram.h>
#endif

#include <Servo.h> 
#include <ros.h>
#include <std_msgs/UInt16.h>
#include <sensor_msgs/JointState.h>

ros::NodeHandle  nh;
Servo servo;

double ang=0;

void servo_cb( const sensor_msgs::JointState& cmd_msg){
  ang=radiansToDegrees(cmd_msg.position[0]);
  servo.write(ang); //set servo angle, should be from 0-180  
  digitalWrite(13, HIGH-digitalRead(13));  //toggle led  
}


ros::Subscriber<sensor_msgs::JointState> sub("joint_states", servo_cb);

void setup(){
  pinMode(13, OUTPUT);
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  nh.subscribe(sub);

  servo.attach(9); //attach it to pin 9
}

void loop(){
  nh.spinOnce();
}

double radiansToDegrees(float position_radians)
{

  position_radians = position_radians + 1.6;

  return position_radians * 57.2958;

}