#include <SPI.h>
#include <Servo.h>
#include <Wire.h>

Servo sL;
Servo sR;
Servo sM;

int sLpin = 22;
int sRpin = 23;
int sMpin = 24;

int Lref = 92;//92 - stop; <92 - back; >92 - forward
int Rref = 92;//92 - stop; >92 - back; <92 - forward
int Mref = 90;



#define XSensorPin A1
#define YSensorPin A2

const byte SlaveDeviceId = 1;
byte LastMasterCommand = 0;


char str[4];

void setup()
{
  sL.attach(sLpin);
  sR.attach(sRpin);
  //sM.attach(sMpin);

  sL.write(Lref);
  sR.write(Rref);
  //sM.write(Mref);
  //delay(2000);

  Wire.begin(8);                // join i2c bus with address #8
  Wire.onReceive(receiveEvent); // register event
  Serial.begin(9600);           // start serial for output
}





int x;    //variable x determine whether led on master is on or off//





void loop()
{
  delay(100);
}

String ss;
String cc;
int a = 0;
int b = 0;


char c;
int aa;
int bb;
int dd;





void receiveEvent(int howMany) {
  ss = "";

  while (1 < Wire.available()) { // loop through all but the last


    c = Wire.read(); // receive byte as a character
    ss = ss + char(c);
    c = Wire.read(); // receive byte as a character
    ss = ss + char(c);
    c = Wire.read(); // receive byte as a character
    ss = ss + char(c);
    c = Wire.read(); // receive byte as a character
    ss = ss + char(c);
    aa = ss.toInt();
  //  Serial.println(aa);
    ss = "";

    c = Wire.read(); // receive byte as a character
    ss = ss + char(c);
    c = Wire.read(); // receive byte as a character
    ss = ss + char(c);
    c = Wire.read(); // receive byte as a character
    ss = ss + char(c);
    c = Wire.read(); // receive byte as a character
    ss = ss + char(c);
    bb = ss.toInt();
    //Serial.println(bb);
    ss = "";


    c = Wire.read(); // receive byte as a character
    ss = ss + char(c);
    c = Wire.read(); // receive byte as a character
    ss = ss + char(c);
    c = Wire.read(); // receive byte as a character
    ss = ss + char(c);
    c = Wire.read(); // receive byte as a character
    ss = ss + char(c);
    dd = ss.toInt();
   // Serial.println(dd);
    ss = "";
  }
  char d = Wire.read(); // receive byte as a character
 // Serial.println("---");
  int x = Wire.read();    // receive byte as an integer


aa=map(aa,30,1000,10,-10);
Serial.println(aa);

bb=map(bb,30,1000,10,-10);
//Serial.println(bb);

dd=map(dd,0,1024,0,180);

 sL.write(Lref+bb);
 sR.write(Rref-bb);

if (aa<0)//RIGHT TURN
{
 // sL.write(Lref+bb);
 //sR.write(Rref-bb+aa);

   sL.write(Lref+bb);
 sR.write(Rref-bb-aa);
  }

if (aa>0)//LEFT TURN
{ sL.write(Lref+bb-aa);
 sR.write(Rref-bb);
  }

 //sL.write(Lref-aa);
 //sR.write(Rref-bb);




//Serial.println(Lref+bb);


//int Lref = 92;//92 - stop; <92 - back; >92 - forward
//int Rref = 92;//92 - stop; >92 - back; <92 - forward
//int Mref = 90;




}