OBSTACLE_AVOIDING_ROBOT_Ahmed_Alhammali.ino

          // OBSTACLE AVOIDANCE FOR MOBILE ROBOT //
               // Author: Ahmed Alhammali //

// You have to install these Library:- 
// AFMotor Library , NewPing Library , Servo Library.
// you can find these Libraries in Sketch -->  Include Library --> Manage Libraries 


#include <AFMotor.h>  
#include <NewPing.h>
#include <Servo.h> 

#define TrigPin A0 // Trig pin in the ultrasonic sensor soldered to the A0 pin on the Motor shield.
#define EchoPin A1 // Echo pin in the ultrasonic sensor soldered to the A1 pin on the Motor shield.
#define Maximum_Dis 250 // maximum useable sensor measuring distance has been set to 250 cm.
#define MaximumSpeed 130 // sets speed of DC  motors
#define MaximumSpeed_Offset 40  // maximum speed OFFSET of the DC Motors have been set to 40

NewPing sonar(TrigPin, EchoPin, Maximum_Dis); // sets up the sonar from NewPing library to use the correct pins to measure distance.

AF_DCMotor M1(1, MOTOR12_1KHZ); // sets Motor #1 to 1kHZ PWM Frequency using AF_DCMotor at AFMotor Library. 
AF_DCMotor M2(2, MOTOR12_1KHZ); // sets Motor #2 to 1kHZ PWM Frequency using AF_DCMotor at AFMotor Library.
AF_DCMotor M3(3, MOTOR34_1KHZ); // sets Motor #3 to 1kHZ PWM Frequency using AF_DCMotor at AFMotor Library.
AF_DCMotor M4(4, MOTOR34_1KHZ); // sets Motor #4 to 1kHZ PWM Frequency using AF_DCMotor at AFMotor Library.
Servo New_Servo;     // create a New_servo object to control the servo

int Measure_Distance = 0 , Set_Speed = 0;
boolean going_Forward = false;

//****************************************** SETUP LOOP *************************************************************************
void setup() {

  New_Servo.attach(10);  // attaches the servo on pin 10
  New_Servo.write(115); // sets the servo to position at 115 degrees to face the servo forward.
  delay(2000);             // delay for Two seconds
  Measure_Distance = Read_Sensor_Ping(); // read the ultrasonic sensor Ping and put the result into Measure_Distance variable. 
  delay(100);
  Measure_Distance = Read_Sensor_Ping();
  delay(100);
  Measure_Distance = Read_Sensor_Ping();
  delay(100);
  Measure_Distance = Read_Sensor_Ping();
  delay(100);
}

//****************************************** MAIN LOOP *************************************************************************

void loop()
{
  
 delay(50);
 Measure_Distance = Read_Sensor_Ping(); 
 
 if(Measure_Distance >= 22)  // move forward if Distance greater than 22 cm.
 {
  Move_Robot_Forward();
 }
 else
 {
  Change_Robot_Direction(); // if destance less than 22 cm change direction.
 }
 
}

//****************************************** CHANGE DIRECTION FUNCTION *************************************************************************

void Change_Robot_Direction()
{
  
int Measure_Distance_Right = 0 , Measure_Distance_Left =  0;
 delay(50);
 
Stop_Moving_Robot();   // stop moving the robot.
  delay(120);          
  Move_Robot_Backward(); // move the robot backward
  delay(320);
  Stop_Moving_Robot();  // stop moving the robot.
  delay(220);
  Measure_Distance_Right = Head_Look_Right(); // check the distance on the right.
  delay(220);
  Measure_Distance_Left = Head_Look_Left();   // check the distance on the left.
  delay(220);

  if(Measure_Distance_Right >= Measure_Distance_Left) //if right is less or equal obstructed.
  {
    Turn_Robot_Right();   // turn the robot to the right.
    Stop_Moving_Robot();
  }else                   //if left is less obstructed
  {
    Turn_Robot_Left();    // turn the robot to the left.
    Stop_Moving_Robot();
  }

}


//****************************************** READ ULTRASONIC SENSOR FUNCTION *************************************************************************

int Read_Sensor_Ping()      // read the ultrasonic sensor distance and return it in cm
{ 
  delay(80);
  int cm = sonar.ping_cm();
  if(cm==0)
  {
    cm = 250;
  }
  return cm;
}

//****************************************** MOVE FORWARD FUNCTION *************************************************************************
  
void Move_Robot_Forward()
{

 if(!going_Forward)
  {                    // if the going forward is not true, make it ture and move the robot forward.
    going_Forward = true;
    M1.run(FORWARD);      
    M2.run(FORWARD);
    M3.run(FORWARD); 
    M4.run(FORWARD);     
   for (Set_Speed = 0; Set_Speed < MaximumSpeed; Set_Speed +=2) 
   {                   // slowly bring the speed up to avoid loading down the batteries too quickly.
    M1.setSpeed(Set_Speed);
    M2.setSpeed(Set_Speed);
    M3.setSpeed(Set_Speed);
    M4.setSpeed(Set_Speed);
    delay(10);
   }
  }
}

//****************************************** MOVE BACKWARD FUNCTION *************************************************************************

void Move_Robot_Backward()
{
    going_Forward=false;  // make sure the going forward is false and move the robot backward.
    M1.run(BACKWARD);      
    M2.run(BACKWARD);
    M3.run(BACKWARD);
    M4.run(BACKWARD);  
  for (Set_Speed = 0; Set_Speed < MaximumSpeed; Set_Speed +=2) 
  {                      // slowly bring the speed up to avoid loading down the batteries too quickly.
    M1.setSpeed(Set_Speed);
    M2.setSpeed(Set_Speed);
    M3.setSpeed(Set_Speed);
    M4.setSpeed(Set_Speed);
    delay(10);
  }
}  


//****************************************** STOP MOVE FUNCTION *************************************************************************

void Stop_Moving_Robot()
{
  M1.run(RELEASE); 
  M2.run(RELEASE);
  M3.run(RELEASE);
  M4.run(RELEASE);
  } 

//****************************************** LOOK RIGHT FUNCTION *************************************************************************


int Head_Look_Right()
{
    New_Servo.write(50); // sets the position of the servo to the right
    delay(600);
    int Measure_Distance = Read_Sensor_Ping();  //sets right distance
    delay(150);
    New_Servo.write(115);    //return the head to center
    return Measure_Distance;
    delay(150);
}

//****************************************** LOOK LEFT FUNCTION *************************************************************************

int Head_Look_Left()
{
    New_Servo.write(170);    // sets the position of the servo to the left
    delay(600);
    Measure_Distance = Read_Sensor_Ping();  //sets left distance
    delay(150);
    New_Servo.write(115); //return the head to center
    return Measure_Distance; 
    delay(150);
}


//****************************************** TURN RIGHT FUNCTION *************************************************************************

void Turn_Robot_Right() 
{
  M1.run(FORWARD);           // Run Motor 1 FORWARD.
  M2.run(FORWARD);           // Run Motor 2 FORWARD.
  M3.run(BACKWARD);          // Run Motor 3 BACKWARD.
  M4.run(BACKWARD);          // Run Motor 4 BACKWARD.
  delay(550);                // Run The Motors for 550 m seconds
  M1.run(FORWARD);      
  M2.run(FORWARD);
  M3.run(FORWARD);
  M4.run(FORWARD);      
} 

//****************************************** TURN LEFT FUNCTION *************************************************************************

void Turn_Robot_Left()
{
  M1.run(BACKWARD);     
  M2.run(BACKWARD);  
  M3.run(FORWARD);
  M4.run(FORWARD);   
  delay(550);
  M1.run(FORWARD);     
  M2.run(FORWARD);
  M3.run(FORWARD);
  M4.run(FORWARD);
}