-
Notifications
You must be signed in to change notification settings - Fork 11
/
Copy pathultrasonic sensor code
168 lines (145 loc) · 4.14 KB
/
ultrasonic sensor code
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
159
160
161
162
163
164
165
166
167
/*#####Parts#####
* Motor Shield (L298N)
* Ultrasonic sensor (HC-SR04)
*#####Pins#####
* 5 --> ENA
* 6 --> ENB
* 2 --> IN1
* 3 --> IN2
* 4 --> IN3
* 7 --> IN4
* 8 --> Trig
* 9 --> Echo
*/
#include <Servo.h>
Servo servo1; // Create object servo1
const int TrigPin=8; //trigger pin for the sensor
const int ReciPin=9; // echo pin for data recive from the sensor
const int DangerT=10; // min distange to the wall
// Left side
int ENA=5; //ENA anf ENB controlls speed of the motors
int IN1=2;
int IN2=3;
/*Here is the controlls for the motors
* IN1: HIGH; IN2: LOW --> Direction1 (Left side forward)
* IN1: LOW; IN2: HIGH --> Direction2 (left side backward
* IN3: HIGH; IN4: LOW --> Direction1 (right side forward)
* IN3: LOW; IN4: HIGH --> Direction2 (right side backward)
*/
//right side
int IN3=4;
int IN4=7;
int ENB=6;
int Rdis; //variable for right side sensor value
int Ldis; //variable for left side sensor value
long duration; //distange to wall in centimetres
int motorSpeed=80; // speed for going forward
int motorTurnSpeed=110; // speed for turning
void setup()
{
servo1.attach(10);
servo1.write(90);
pinMode(ENA,OUTPUT); //set pin modes
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(ENB,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
pinMode(TrigPin,OUTPUT);
pinMode(ReciPin,INPUT);
}
void loop()
{
int distange = ping();
if(distange>DangerT)// go forward if there isn't wall in front of the car
{
void forward();
}
else //if there is wall
{
void motorStop();
servo1.write(0); //turn the servo to right
delay(500); // wait for the servo to turn
Rdis = ping(); //ping right and save it to var Rdis
delay(100); // wait for the sensor to ping
servo1.write(180); // turn servo left
delay(700); //wait longer because the servo has to turn 180 degrees
Ldis = ping(); //ping left and save it to var Ldis
delay(100); // wait for the sensor to ping
servo1.write(90); // senter the servo
delay(500); //wait for the servo to senter
cdis(); // goto compare distange
}
}
void cdis()
{
if(Ldis>DangerT) // if left is more than DangerT (10)
{
left(); // turn left
delay(500); // wait for the car to turn (you might need to adjust these timings for tunrnig)
motorStop(); // stop
}
else if(Rdis>DangerT) // if left where less than DangerT (10) check if right is less than DangerT (10)
{
right(); // turn right
delay(500); //wait for the car to turn
motorStop(); // stop
}
else // if right and left where less than DangerT turn around
{
left(); //turn left
delay(1000); //wait for the car to turn around
motorStop();//stop
}
}
long ping()
{
digitalWrite(TrigPin,LOW);//set the pn to low just in case
delayMicroseconds(2); //wait for it to turn
digitalWrite(TrigPin,HIGH); //set it high for 5 microseconds
delayMicroseconds(5);
digitalWrite(TrigPin,LOW);//and back to low again
duration = pulseIn(ReciPin,HIGH); // measure the time it took the sound to bounce back
return duration / 29 / 2; // calculate the distange (in centimetres)
}
void forward() // from here starts the motor controlls at the start if this sketch there where instructions on how to control the motors
{
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
analogWrite(ENA,motorSpeed);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
analogWrite(ENB,motorSpeed);
}
void backward()
{
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
analogWrite(ENA,motorSpeed);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
analogWrite(ENB,motorSpeed);
}
void left()
{
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
analogWrite(ENA,motorTurnSpeed);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
analogWrite(ENB,motorTurnSpeed);
}
void right()
{
analogWrite(ENA,motorTurnSpeed);
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
analogWrite(ENB,motorTurnSpeed);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
}
void motorStop()
{
analogWrite(ENA,0);
analogWrite(ENB,0);
}