-
Notifications
You must be signed in to change notification settings - Fork 0
/
obstacle.ino
76 lines (69 loc) · 1.67 KB
/
obstacle.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
//FB motor - A2, A3
//LR motor - A0, A1
//servo (0, 90, 180) = (right, front, left)
#define ENA 3
#define ENB 5
#define trig 10
#define echo 11
#define ser 12
#include<Servo.h>
Servo servo;
int angle = 90;
long duration;
int distance;
int x, y, z;
int d;
int dir;
void setup() {
pinMode(2, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(A0, OUTPUT);
pinMode(A1, OUTPUT);
pinMode(A2, OUTPUT);
pinMode(A3, OUTPUT);
pinMode(trig, OUTPUT);
pinMode(echo, INPUT);
servo.attach(ser);
servo.write(angle);
digitalWrite(ENB, LOW);
digitalWrite(2, HIGH);
}
int ping() {
digitalWrite(10, LOW); delayMicroseconds(2); digitalWrite(10, HIGH); delayMicroseconds(10); digitalWrite(10, LOW);
duration = pulseIn(11, HIGH); distance = duration * 0.034 / 2;
return distance;
}
void backward() {digitalWrite(ENA, HIGH); digitalWrite(A2, HIGH); digitalWrite(A3, LOW);}
void forward() {digitalWrite(ENA, HIGH); digitalWrite(A2, LOW); digitalWrite(A3, HIGH);}
void left() {digitalWrite(ENB, HIGH); digitalWrite(A0, HIGH); digitalWrite(A1, LOW);}
void right() {digitalWrite(ENB, HIGH); digitalWrite(A0, LOW); digitalWrite(A1, HIGH);}
void br() {digitalWrite(ENA, LOW); digitalWrite(A2, LOW); digitalWrite(A3, LOW);}
int sweep() {
servo.write(0); x=ping();
delay(1500);
servo.write(90); y=ping();
delay(1500);
servo.write(180); z=ping();
delay(1500);
servo.write(90);
if (x<z){return 1;}else if (z<x){return 2;}
}
void loop() {
delay(500);
d = ping();
if(d>20) {
forward();
delay(2000);
} else if(d<20) {
br();
dir = sweep();
if (dir==1) {
left();
forward();
}else if(dir == 2) {
right();
forward();
}
}
}