/*
  Robot Obstacle Avoidance

 */

int ENA1=2;   /* turn on/off left motor */
int IN1=3;    /* control for left motor */
int IN2=4;    /* control for left motor */

int ENA2 = 7; /* turn on/off right motor */
int IN3 = 5;  /* control for right motor */
int IN4 = 6;  /* control for right motor */


// the setup routine runs once when you press reset:
void setup() {
 Serial.begin(9600);
 pinMode(IN1,OUTPUT);
 pinMode(IN2,OUTPUT);
 pinMode(ENA1,OUTPUT);

 pinMode(IN3,OUTPUT);
 pinMode(IN4,OUTPUT);
 pinMode(ENA2,OUTPUT);
}

// the loop routine runs over and over again forever:
void loop() {
/* direction:
   forward: low/high
   reverse: high/low
   stop:    low/low
*/

MoveForward(128,50);
TurnRight(128,50);
MoveForward(128,50);
TurnRight(128,50);

delay(1000);

MoveBackward(128,50);
TurnLeft(128,50);
MoveBackward(128,50);
TurnLeft(128,50);


}

void MoveForward(int speed, int tSec)
{
   digitalWrite(IN1,LOW);
   digitalWrite(IN2,HIGH);
   digitalWrite(IN3,LOW);
   digitalWrite(IN4,HIGH);
   MoveRobot(speed, tSec);
}

void MoveBackward(int speed, int tSec)
{
   digitalWrite(IN1,HIGH);
   digitalWrite(IN2,LOW);
   digitalWrite(IN3,HIGH);
   digitalWrite(IN4,LOW);
   MoveRobot(speed, tSec);
}

void TurnRight(int speed, int tSec)
{
   digitalWrite(IN1,LOW);
   digitalWrite(IN2,HIGH);
   digitalWrite(IN3,HIGH);
   digitalWrite(IN4,LOW);
   MoveRobot(speed, tSec);
}

void TurnLeft(int speed, int tSec)
{
   digitalWrite(IN1,HIGH);
   digitalWrite(IN2,LOW);
   digitalWrite(IN3,LOW);
   digitalWrite(IN4,HIGH);
   MoveRobot(speed, tSec);
}

void MoveRobot(int motorSpeed, int tSec)
{
   analogWrite(ENA1, motorSpeed);// motor speed
   analogWrite(ENA2, motorSpeed);// motor speed
   // Serial.println("Moving");
   delay(tSec * 10);
   // Serial.println("Stopping");
   analogWrite(ENA1, 0);
   analogWrite(ENA2, 0);
}