-
Notifications
You must be signed in to change notification settings - Fork 0
/
obstacleAvoidance.c
144 lines (113 loc) · 3.07 KB
/
obstacleAvoidance.c
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
144
#include <HCSR04.h>
#include <Servo.h>
// Define Motor Pin
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 Ultrasonic Sensor Pin
#define Echo 7
#define Trig 8
// Variable to store distance
int Front_D = 0;
int Left_D = 0;
int Right_D = 0;
int Max_D = 50; // Max distance to obastacle
UltraSonicDistanceSensor distanceSensor(Trig, Echo); // Initialize sensor
Servo Servo_1; // create servo object to control a servo
int pos = 0; // variable to store the servo position
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
// Define Motor Pin as output
pinMode(Motor_L_F, OUTPUT);
pinMode(Motor_L_B, OUTPUT);
pinMode(Motor_R_F, OUTPUT);
pinMode(Motor_R_B, OUTPUT);
Servo_1.attach(9); // attaches the servo on pin 9 to the servo object
int pos = 90;
Servo_1.write(pos);
delay(1000);
}
void loop() {
// put your main code here, to run repeatedly:
Front_D = distanceSensor.measureDistanceCm(); // measuring fornt distance
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(){
// Run Left Motor In Forward Direction
digitalWrite(Motor_L_F, HIGH);
digitalWrite(Motor_L_B, LOW);
//Run Right Motor in Forward Direction
digitalWrite(Motor_R_F, HIGH);
digitalWrite(Motor_R_B, LOW);
}
void Right(){
//Stop Right Motor
digitalWrite(Motor_R_F, LOW);
digitalWrite(Motor_R_B, LOW);
//Run Left Motor in Forward Direction
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);
}