#define leftp 7
#define leftn 4
#define rightp 3
#define rightn 2
#define speedleft 5
#define speedright 6
#define surfaceSense1 9
#define surfaceSense2 8

//Function prototypes

void straight(int speedpwm);
void left(int speedpwm);
void right(int speedpwm);
void reverse(int speedpwm);
void brake(int speedpwm);
void readSurface();

void setup() {
  Serial.begin(9600);
  pinMode(leftp, OUTPUT);
  pinMode(leftn, OUTPUT);
  pinMode(rightp, OUTPUT);
  pinMode(rightn, OUTPUT);
  pinMode(speedleft, OUTPUT);
  pinMode(speedright, OUTPUT);
  pinMode(surfaceSense1, INPUT);
  pinMode(surfaceSense2, INPUT);


void straight(int speedpwm){
  digitalWrite(leftp, HIGH);
  digitalWrite(leftn, LOW);
  digitalWrite(rightp, HIGH);
  digitalWrite(rightn, LOW);
  analogWrite(speedleft, speedpwm);
  analogWrite(speedright, speedpwm); 
  }

void left(int speedpwm){
  digitalWrite(leftp, LOW);
  digitalWrite(leftn, LOW);
  digitalWrite(rightp, HIGH);
  digitalWrite(rightn, LOW);  
  analogWrite(speedleft, speedpwm);
  analogWrite(speedright, speedpwm); 
  }

void right(int speedpwm){
  digitalWrite(leftp, HIGH);
  digitalWrite(leftn, LOW);
  digitalWrite(rightp, LOW);
  digitalWrite(rightn, LOW);
  analogWrite(speedleft, speedpwm);
  analogWrite(speedright, speedpwm); 
  }

void reverse(int speedpwm){
  digitalWrite(leftp, LOW);
  digitalWrite(leftn, HIGH);
  digitalWrite(rightp, LOW);
  digitalWrite(rightn, HIGH);
  analogWrite(speedleft, speedpwm);
  analogWrite(speedright, speedpwm); 
  }
void brake(int speedpwm){
  digitalWrite(leftp, LOW);
  digitalWrite(leftn, LOW);
  digitalWrite(rightp, LOW);
  digitalWrite(rightn, LOW);
  analogWrite(speedleft, speedpwm);
  analogWrite(speedright, speedpwm); 
  }

void readSurface(){
  //For left sensor
  if(digitalRead(surfaceSense1) == HIGH){
    //Check if both sensor was triggered simultaneously
    if(digitalRead(surfaceSense2) == HIGH){
      //Robot backs up for a bit before making appropriate turning
      reverse(255);
      delay(1000); 
      }
    //Here the robot avoids the line from the left
    right(255);
    delay(500);
    //Keeps executing until the line was cleared from sensor
    readSurface();
    }
  //For right sensor
  else if(digitalRead(surfaceSense2) == HIGH){
    //Check if both sensor was triggered simultaneously
    if(digitalRead(surfaceSense2) == HIGH){
      //Robot backs up for a bit before making appropriate turning
      reverse(255);
      delay(1000); 
      }
    //Here the robot avoids the line from the right
    left(255);
    delay(500);
    //Keeps executing until the line was cleared from sensor
    readSurface();
  }