   len = abs(msg.data);
   motion(lpwm,rpwm,LOW,LOW,len);
 }
-void angleCallback(const std_msgs::Int16& msg) {
-  i = abs(msg.data);
-  s.write(i);
+
+int sstep(){
+  sa+=sd;
+  if(sa>smax || sa<smin) sd=-sd;
+  s.write(sa);
 }
 int srange(){  // calculate distance from ultrasonic sensor
   long duration, distance;

  nh.advertise(leftenc);
  nh.advertise(rightenc);
  nh.advertise(range);
  nh.subscribe(sub_r);
  nh.subscribe(sub_l);
  nh.subscribe(sub_f);
  nh.subscribe(sub_b);
  nh.subscribe(sub_a);

  pinMode(D0, OUTPUT); // Ultrasonic Trigger
  pinMode(D1, OUTPUT); // 1,2EN aka D1 pwm left

ros::Publisher leftenc("/car/lencoder", &int_msg);
ros::Publisher rightenc("/car/rencoder", &int_msg);
ros::Publisher range("/car/range", &int_msg);
ros::Publisher angle("/car/angle", &int_msg);

// ROS SUBSCRIBERS
ros::Subscriber<std_msgs::Int16> sub_f("/car/forward", &forwardCallback);
ros::Subscriber<std_msgs::Int16> sub_b("/car/backward", &backwardCallback);
ros::Subscriber<std_msgs::Int16> sub_l("/car/left", &leftCallback);
ros::Subscriber<std_msgs::Int16> sub_r("/car/right", &rightCallback);

double x = 1.0;
double y = 0.0;
