/*##### Motor Shield (L298N) #####*/
/*
* Arduino Pin --> L298N
* 5 --> ENA
* 6 --> ENB
* 2 --> IN1
* 3 --> IN2
* 4 --> IN3
* 7 --> IN4
*/
const int ENA = 5;
const int ENB = 6; 
/*
* IN1: HIGH; IN2: LOW --> Direction 1 
* IN1: LOW; IN2: HIGH --> Direction 2
* IN3: HIGH; IN4: LOW --> Direction 1
* IN3: LOW; IN4: HIGH --> Direction2
*/
const int IN1 = 11; 
const int IN2 = 10;
const int IN3 = 9;
const int IN4 = 8;

const int leftTrigger=0;
const int leftEcho=1;

const int forTrigger =2;
const int forEcho=3;

const int rightTopTrigger = 4;
const int rightTopEcho =5;

const int rightBottomTrigger = 7;
const int rightBottomEcho = 13;

void setup()
{
 pinMode(ENA, OUTPUT);
 pinMode(ENB, OUTPUT);
 pinMode(IN1, OUTPUT);
 pinMode(IN2, OUTPUT);
 pinMode(IN3, OUTPUT);
 pinMode(IN4, OUTPUT);
 pinMode(leftTrigger, OUTPUT);
 pinMode(leftEcho, INPUT);
 pinMode(forTrigger, OUTPUT);
 pinMode(forEcho, INPUT);
 pinMode(rightTopTrigger, OUTPUT);
 pinMode(rightTopEcho, INPUT);
  pinMode(rightBottomTrigger, OUTPUT);
 pinMode(rightBottomEcho, INPUT);
 // Enable Motor A, Motor B: Constant Speed
 digitalWrite(ENA, HIGH);
 digitalWrite(ENB, HIGH);
 // Serial communication
 Serial.begin(9600);
}
 
void loop()
{
  if(getForDistance()>30)
  {
    MotorAB_Direction1(1000);
    MotorAB_Brake(1000);
  }
  else
  {
    turnLeft(1000);
  }
}


 long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2;
}

void MotorAB_Direction1(int milliseconds)
{
  analogWrite(IN1,200);
   analogWrite(IN2, 100);
   analogWrite(IN3, 200);
   analogWrite(IN4,100);
 if (milliseconds > 0)
 delay(milliseconds);
}
 
void MotorAB_Direction2(int milliseconds)
{
  analogWrite(IN1,100);
   analogWrite(IN2, 200);
   analogWrite(IN3, 100);
   analogWrite(IN4,200);
 if(milliseconds > 0)
 delay(milliseconds);
}
 
void MotorAB_Brake(int milliseconds)
{
  analogWrite(IN1,100);
   analogWrite(IN2, 100);
   analogWrite(IN3, 100);
   analogWrite(IN4,100);
 if(milliseconds > 0)
 delay(milliseconds); 
}

void turnLeft(int milliSeconds)
{
   analogWrite(IN1,200);
   analogWrite(IN2, 100);
   analogWrite(IN3, 100);
   analogWrite(IN4,200);
   if(milliSeconds > 0)
     delay(milliSeconds);   
}

void turnRight(int milliSeconds)
{
   analogWrite(IN1, 100);
   analogWrite(IN2, 200);
   analogWrite(IN3, 200);
   analogWrite(IN4, 100);
   if(milliSeconds > 0)
     delay(milliSeconds);   
}


 int getForDistance() 
 {
  digitalWrite(forTrigger,HIGH);
  int duration = pulseIn(forEcho,HIGH);
  Serial.print("    duration:    ");
  Serial.println(duration);
  
  int cmDistance = microsecondsToCentimeters(duration);
  Serial.print("    jjj    ");
  Serial.println(cmDistance);
  return cmDistance;
 }

int getRightTopDistance() 
 {
  digitalWrite(rightTopTrigger,HIGH);
  int duration = pulseIn(rightTopEcho,HIGH);
  int cmDistance = microsecondsToCentimeters(duration);
  return cmDistance;
 }

 
int getRightBottomDistance() 
 {
  digitalWrite(rightTopTrigger,HIGH);
  int duration = pulseIn(rightTopEcho,HIGH);
  int cmDistance = microsecondsToCentimeters(duration);
  return cmDistance;
 }
 

void stickToRight()
{
  while((getRightTopDistance()<30)&&(getRightBottomDistance()<30))
  {
    if(getRightTopDistance()<30)
  {
    turnLeft(100);
  }
  if(getRightBottomDistance()<30)
  {
    turnRight(100);
  } 
  }
 
}