//To control the rover
const int E1 = 6; //M1 Speed Control
const int E2 = 5; //M2 Speed Control
const int M1 = 8; //M1 Direction Control
const int M2 = 7; //M2 Direction Control

//LED
int loopPin = 12;

//Interrupt Pin
int interruptPin = 2;

//Global Variables:
const int leftspeed = 255;
const int rightspeed = 255;
bool inService = false;
const char reply = '1';


void setup() {
  // put your setup code here, to run once:
 Serial.begin(9600);


  //Declare Rover Motor Pins
  int i;
  for(i=5;i<=8;i++)
  {
    pinMode(i, OUTPUT);
  }

 // Declare LED pin used to signal that main loop is alive
 pinMode(loopPin, OUTPUT);
 digitalWrite(loopPin, LOW);

 // Using interrupt 0 on digital pin 2.
 pinMode(interruptPin, INPUT);
 digitalWrite(interruptPin, LOW);
 attachInterrupt(0, serialInterrupt, CHANGE);

}


void loop() {
  // put your main code here, to run repeatedly:
  digitalWrite(loopPin, HIGH);
}


void serialInterrupt()
{
 // Already in service? Do nothing.
 if (inService) return;

 // If not in Service, Flag.
 inService = true;

 // Reenable interrupts to Receive Data and Use Delay()
 interrupts();

  // stop any current movement
 stop();

 //Show inservice
 digitalWrite(loopPin, LOW);

 // Allow serial to read
 while(!Serial.available()){}

 //Read Data
 char data = Serial.read();

 //Make Decision on Received Data
 switch(data)
 {
  case 'w'://Move Forward
    forward (leftspeed,rightspeed);
    //range(cm);  if (cm <= 5){exit(1);}
    break;
  case 'a'://Turn Left
    left (leftspeed,rightspeed);
    // range(cm);  if (cm <= 5){exit(1);}
    break;
  case 'd'://Turn Right
    right (leftspeed,rightspeed);
    //range(cm);  if (cm <= 5){exit(1);}
    break;
  default:
    stop();
    // range(cm);  if (cm <= 5){exit(1);}
    break;
 }

 // Interrupt Routine Finished
 //Reply to Pi
 Serial.write(reply);
 Serial.flush();

 //Allow New Interrupt
 inService = false;
}

//Rover Control Functions
void stop(void) //Stop
{
 digitalWrite(E1,LOW);
 digitalWrite(E2,LOW);
}

void forward(char a,char b)
{
 analogWrite (E1,a);
 digitalWrite(M1,LOW);
 analogWrite (E2,b);
 digitalWrite(M2,LOW);
 delay(200);
 stop();
 delay(50);
}

void left (char a,char b)
{
 analogWrite (E1,a);
 digitalWrite(M1,HIGH);
 analogWrite (E2,b);
 digitalWrite(M2,LOW);
 delay(200);
 stop();
 delay(50);
}
void right (char a,char b)
{
 analogWrite (E1,a);
 digitalWrite(M1,LOW);
 analogWrite (E2,b);
 digitalWrite(M2,HIGH);
 delay(200);
 stop();
 delay(50);
}