diff --git a/OBSTACLE_AVOIDING_ROBOT_Ahmed_Alhammali.ino b/OBSTACLE_AVOIDING_ROBOT_Ahmed_Alhammali.ino new file mode 100644 index 0000000..eb231d5 --- /dev/null +++ b/OBSTACLE_AVOIDING_ROBOT_Ahmed_Alhammali.ino @@ -0,0 +1,218 @@ + // OBSTACLE AVOIDING ROBOT // + +// You have to install these Library:- +// AFMotor Library , NewPing Library , Servo Library. +// you can find these Libraries in Sketch --> Include Library --> Manage Libraries + + +#include +#include +#include + +#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); +}