ard

#include <ros.h>
#include <std_msgs/String.h>



#include "WString.h"
#include <std_msgs/Empty.h>
#include <Servo.h>
ros::NodeHandle  nh;

double left_signal = 0, right_signal = 0;
void messageCb( const std_msgs::String &msg){
      Serial.println("in message cb");
      sscanf(msg.data, "%lf %lf", &left_signal, &right_signal);
      Serial.println(left_signal);
      Serial.println(right_signal);

}

ros::Subscriber<std_msgs::String> sub("/game_controller", &messageCb );

Servo myservo_right;  // create servo object to control a servo
Servo myservo_left; //left side motor control


int potpin = 0;  // analog pin used to connect the potentiometer
int val;    // variable to read the value from the analog pin

void setup()
{

  Serial.begin(9600);
  myservo_right.attach(5);  // attaches the servo on pin 9 to the servo object
  myservo_left.attach(6);
  nh.initNode();
  nh.subscribe(sub);
}

void loop()
{
  //val = analogRead(potpin);            // reads the value of the potentiometer (value between 0 and 1023)
  //val = map(val, 0, 1023, 0, 179);     // scale it to use it with the servo (value between 0 and 180)
  myservo_right.write(right_signal);  // sets the servo position according to the scaled value
  myservo_left.write(left_signal);
  nh.spinOnce();
  delay(15);
  // waits for the servo to get there
}