-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathObstacleCar.ino
158 lines (152 loc) · 3.69 KB
/
ObstacleCar.ino
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
145
146
147
148
149
150
151
152
153
154
155
156
157
158
#include "L293.h"
#include"Servo.h"
int large_find(int []);
AF_DCMotor rightBack(2); //Create an object to control each motor
AF_DCMotor rightFront(4);
AF_DCMotor leftFront(3);
AF_DCMotor leftBack(1);
Servo servoLook;
byte trig=2;
byte echo=13;
byte motorSpeed=200;
byte maxDist = 150;
int motorOffset = 10; //Factor to account for one side being more powerful
int turnSpeed = 50;
float timeOut=2*(maxDist+10)/100/340*1000000;
void setup() {
rightBack.setSpeed(motorSpeed); //Set the motors to the motor speed
rightFront.setSpeed(motorSpeed);
leftFront.setSpeed(motorSpeed);
leftBack.setSpeed(motorSpeed);
rightBack.run(RELEASE); //Ensure all motors are stopped
rightFront.run(RELEASE);
leftFront.run(RELEASE);
leftBack.run(RELEASE); //Assign the servo pin
pinMode(trig,OUTPUT); //Assign ultrasonic sensor pin modes
pinMode(echo,INPUT);
servoLook.attach(10);
Serial.begin(9600);
}
void loop() {
servoLook.write(90);
rightFront.run(FORWARD);
leftFront.run(FORWARD);
rightBack.run(FORWARD);
leftBack.run(FORWARD); //Create a variable to store the pulse travel time
int distance;
int distances[4]={0,0,0,0};
distance=get_distance(); //Create a variable to store the calculated distance
if(distance<=15)
{
rightBack.run(RELEASE); //Ensure all motors are stopped
rightFront.run(RELEASE);
leftFront.run(RELEASE);
leftBack.run(RELEASE);
delay(200);
rightBack.run(BACKWARD);
rightFront.run(BACKWARD);
leftFront.run(BACKWARD);
leftBack.run(BACKWARD);
delay(500);
rightBack.run(RELEASE); //Ensure all motors are stopped
rightFront.run(RELEASE);
leftFront.run(RELEASE);
leftBack.run(RELEASE);
servoLook.write(180); //left
delay(750);
distances[0]=get_distance(); //left
servoLook.write(135); //left-right
delay(400);
distances[1]=get_distance(); //left-right
servoLook.write(45); //right-left
delay(750);
distances[2]=get_distance(); //right-left
servoLook.write(0); //right side
delay(400);
distances[3]=get_distance(); //right
int lg=large_find(distances);
if(lg==0) //condition for turn left side
{
turn_left(700);
}
else if(lg==1) //condition for turn right
{
turn_left(350);
}
else if(lg==2)
{
turn_right(350);
}
else if(lg==3)
{
turn_right(700);
}
}
}
void turn_left(int duration)
{
rightBack.setSpeed(motorSpeed+turnSpeed); //Set the motors to the motor speed
rightFront.setSpeed(motorSpeed+turnSpeed);
leftFront.setSpeed(motorSpeed);
leftBack.setSpeed(motorSpeed);
rightBack.run(FORWARD);
rightFront.run(FORWARD);
leftFront.run(BACKWARD);
leftBack.run(BACKWARD);
delay(duration);
rightBack.setSpeed(motorSpeed); //Set the motors to the motor speed
rightFront.setSpeed(motorSpeed);
leftFront.setSpeed(motorSpeed);
leftBack.setSpeed(motorSpeed);
rightBack.run(RELEASE);
rightFront.run(RELEASE);
leftFront.run(RELEASE);
leftBack.run(RELEASE);
}
void turn_right(int duration)
{
rightBack.setSpeed(motorSpeed); //Set the motors to the motor speed
rightFront.setSpeed(motorSpeed);
leftFront.setSpeed(motorSpeed+turnSpeed);
leftBack.setSpeed(motorSpeed+turnSpeed);
rightBack.run(BACKWARD);
rightFront.run(BACKWARD);
leftFront.run(FORWARD);
leftBack.run(FORWARD);
delay(duration);
rightBack.setSpeed(motorSpeed); //Set the motors to the motor speed
rightFront.setSpeed(motorSpeed);
leftFront.setSpeed(motorSpeed);
leftBack.setSpeed(motorSpeed);
rightBack.run(RELEASE);
rightFront.run(RELEASE);
leftFront.run(RELEASE);
leftBack.run(RELEASE);
}
int get_distance()
{
int distances;
unsigned long pulseTime;
digitalWrite(trig, HIGH); //Generate a 10 microsecond pulse
delayMicroseconds(10);
digitalWrite(trig, LOW);
pulseTime = pulseIn(echo, HIGH, timeOut);
distances = (float)pulseTime * 340 / 2 / 10000;
return distances;
}
int large_find(int distances[])
{
int lg=0;
int lg_dis=distances[0];
int i=0;
while(i<4)
{
if(distances[i]>lg_dis)
{
lg_dis=distances[i];
lg=i;
}
i++;
}
return lg;
}