Obstacle-avoiding-bot/Obstacle_Avoiding_Bot.ino at master · Adit-jain/Obstacle-avoiding-bot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143


#include <HCSR04.h>
#include <Servo.h>
const int Motor_L_F = 2;
const int Motor_L_B = 3;
const int Motor_R_F = 4;
const int Motor_R_B = 5;


#define Echo  7
#define Trig  8


int Front_D = 0;
int Left_D = 0;
int Right_D = 0;

int Max_D = 50;


UltraSonicDistanceSensor distanceSensor(Trig, Echo);

Servo Servo_1;
int pos = 0;

void setup() {

  Serial.begin(9600);


  pinMode(Motor_L_F, OUTPUT);
  pinMode(Motor_L_B, OUTPUT);
  pinMode(Motor_R_F, OUTPUT);
  pinMode(Motor_R_B, OUTPUT);

  Servo_1.attach(9);
  int pos = 90;
  Servo_1.write(pos);
  delay(1000);


}

void loop() {

   Front_D = distanceSensor.measureDistanceCm();
   if (Front_D < Max_D)
   {
     Stop();
     Get_D();
     if(Right_D > Max_D)
     {
       Right();
       delay(400);
       Forward();
     }
     else if ( Left_D > Max_D)
     {
       Left();
       delay(400);
       Forward();
     }
     else  {
       Back();
       delay (500);
       Stop();
      }
   }
   else{
    Forward();
   }
}


void Forward(){

  digitalWrite(Motor_L_F, HIGH);
  digitalWrite(Motor_L_B, LOW);

  digitalWrite(Motor_R_F, HIGH);
  digitalWrite(Motor_R_B, LOW);

  }

void Right(){

  digitalWrite(Motor_R_F, LOW);
  digitalWrite(Motor_R_B, LOW);

  digitalWrite(Motor_L_F, HIGH);
  digitalWrite(Motor_L_B, LOW);

  }

void Left(){
  //Stop Left Motor
  digitalWrite(Motor_L_F, LOW);
  digitalWrite(Motor_L_B, LOW);

  //Run Right Motor in Forward Direction
  digitalWrite(Motor_R_F, HIGH);
  digitalWrite(Motor_R_B, LOW);

  }

void Back(){
  // Run Left Motor In Forward Direction
  digitalWrite(Motor_L_F, LOW);
  digitalWrite(Motor_L_B, HIGH);

  //Run Right Motor in Forward Direction
  digitalWrite(Motor_R_F, LOW);
  digitalWrite(Motor_R_B, HIGH);

  }

void Stop(){
  // Run Left Motor In Forward Direction
  digitalWrite(Motor_L_F, LOW);
  digitalWrite(Motor_L_B, LOW);

  //Run Right Motor in Forward Direction
  digitalWrite(Motor_R_F, LOW);
  digitalWrite(Motor_R_B, LOW);

  }

void Get_D(){
    Servo_1.write(0);   // Right Position
    delay(500);
    Right_D = distanceSensor.measureDistanceCm();
    Servo_1.write(90);  // Front Positon
    delay(500);
    Front_D = distanceSensor.measureDistanceCm();
    Servo_1.write(180); // Left position of servo
    delay(500);
    Left_D = distanceSensor.measureDistanceCm();
    Servo_1.write(90);  // back to front
    delay(250);
  }