#include <Servo.h> 
 
Servo myservo1;  // create servo object to control a servo
int pos = 90;    // variable to store the servo position

int E1 = 5; //M1 Speed Control
int E2 = 6; //M2 Speed Control
int M1 = 4; //M1 Direction Control
int M2 = 7; //M1 Direction Control

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

void advance(char a,char b) //Move forward
  {
    analogWrite (E1,a); //PWM Speed Control
    digitalWrite(M1,HIGH);
    analogWrite (E2,b);
    digitalWrite(M2,HIGH);
  }

void back_off (char a,char b) //Move backward
  {
    analogWrite (E1,a);
    digitalWrite(M1,LOW);
    analogWrite (E2,b);
    digitalWrite(M2,LOW);
  }

void turn_L (char a,char b) //Turn Left
  {
    analogWrite (E1,a);
    digitalWrite(M1,LOW);
    analogWrite (E2,b);
    digitalWrite(M2,HIGH);
  }

void turn_R (char a,char b) //Turn Right
  {
    analogWrite (E1,a);
    digitalWrite(M1,HIGH);
    analogWrite (E2,b);
    digitalWrite(M2,LOW);
  }

void setup()
  {
    myservo1.attach(2);
    
    Serial.begin(9600);
    pinMode( A2 , INPUT);
    int i;
    for(i=6;i<=9;i++)
    pinMode(i, OUTPUT);
  }

void loop()
  { 
  for(pos = 0; pos < 180; pos += 1)  // goes from 90 degrees to 120 degrees 
  {                                  // in steps of 1 degree 
    myservo1.write(pos);              // tell servo to go to position in variable 'pos' 
    delay(15);                       // waits 15ms for the servo to reach the position 
  } 
  
  for(pos = 180; pos>=1; pos-=1)     // goes from 180 degrees to 0 degrees 
  {                                
    myservo1.write(pos);              // tell servo to go to position in variable 'pos' 
    delay(15);                       // waits 15ms for the servo to reach the position 
  } 
  
  {
    if (( !( digitalRead( A2) )))//read sensor from IR distence switch
  {
  advance(240,240);
    delay(1000);
    Serial.print( "forward" );
    Serial.println("");
    if (random( 1024 )>512)
  {
  turn_R(240,240);
    delay(1000);
    Serial.print( "random right" );
    Serial.println("");
  }
  else
  {
  turn_L(240,240);
    delay(1000);
    Serial.print( "random left" );
    Serial.println("");
  }
  }
  else
  {
    if (( !( digitalRead( A2) )))//read sensor from IR distence switch
  {
  advance(240,240);
    delay(1000);
  turn_L(240,240);
    delay(1000);
    Serial.print( "forward and left" );
    Serial.println("");
  }
  else
  {
    if (( !( digitalRead( A2) )))//read sensor from IR distence switch
  {
  advance(240,240);
    delay(1000);
  turn_R(240,240);
    delay(1000);
    Serial.print( "forward and right" );
    Serial.println("");
  }
    else
  {
  back_off(240,240);
    delay(1000);
    Serial.print( "go" );
    Serial.println("");
        }
      }
    }
  }
}