1const int trigPin1 = 6;
2const int echoPin1 = 7;
3const int
4 trigPin2 = 10;
5const int echoPin2 = 11;
6const int trigPin3 = 8;
7const
8 int echoPin3 = 9;
9const int in1 = 2;
10const int in2 = 3;
11const int in3 =
12 4;
13const int in4 = 5;
14
15#define DIS 7
16
17void setup()
18{
19pinMode(trigPin1,
20 OUTPUT);
21pinMode(echoPin1, INPUT);
22pinMode(trigPin2, OUTPUT);
23pinMode(echoPin2,
24 INPUT);
25pinMode(trigPin3, OUTPUT);
26pinMode(echoPin3, INPUT);
27pinMode (in1,
28 OUTPUT);
29pinMode (in2, OUTPUT);
30pinMode (in3, OUTPUT);
31pinMode (in4, OUTPUT);
32
33}
34void
35 loop()
36{
37 if (FrontSensor ()>DIS && RightSensor () >DIS && LeftSensor ()
38 >DIS)
39 {forward();}
40
41 DIS && LeftSensor ()< DIS)
42
43
44
45else if ( FrontSensor()
46 < DIS && RightSensor () <DIS && LeftSensor ()<DIS)
47 sides
48{
49reverse ();
50delay(500);
51if((LeftSensor())>(RightSensor()) )
52turn_left();
53else
54turn_right();
55
56 reverse
57}
58else if (FrontSensor() <DIS && RightSensor () <DIS && LeftSensor
59 ()>DIS)
60{
61turn_left ();
62 side
63}
64else if (FrontSensor() <DIS && RightSensor () >DIS && LeftSensor ()<DIS)
65
66{
67turn_right ();
68}
69else
70 if (FrontSensor() <DIS && RightSensor () >DIS && LeftSensor ()>DIS)
71 on front sides
72{
73turn_left ();
74delay(500);
75forward();
76
77}
78else if (FrontSensor() >DIS && RightSensor () >DIS
79 && LeftSensor ()<DIS)
80{
81turn_right();
82 right and then forward
83delay(500);
84forward();
85}
86else if (FrontSensor()
87 >DIS && RightSensor () <DIS && LeftSensor ()>DIS)
88{
89turn_left
90 ();
91delay(500);
92forward();
93}
94else
95{
96forward();
97}
98}
99void
100 forward ()
101{
102digitalWrite(in1, HIGH);
103digitalWrite(in2, LOW);
104digitalWrite(in3,
105 HIGH);
106digitalWrite(in4, LOW);
107}
108void turn_left ()
109{
110digitalWrite(in1,
111 HIGH);
112digitalWrite(in2, LOW);
113digitalWrite(in3, LOW);
114digitalWrite(in4,
115 HIGH);
116
117}
118void turn_right ()
119{
120digitalWrite(in1, LOW);
121digitalWrite(in2,
122 HIGH);
123digitalWrite(in3, HIGH);
124digitalWrite(in4, LOW);
125
126}
127void reverse
128 ()
129{
130digitalWrite(in1, LOW);
131digitalWrite(in2, HIGH);
132digitalWrite(in3,
133 LOW);
134digitalWrite(in4, HIGH);
135
136}
137void stop()
138{
139digitalWrite(in1,
140 LOW);
141digitalWrite(in2, LOW);
142digitalWrite(in3, LOW);
143digitalWrite(in4,
144 LOW);
145
146}
147long FrontSensor ()
148{
149long dur;
150digitalWrite(trigPin1,
151 LOW);
152delayMicroseconds(2);
153digitalWrite(trigPin1,
154 HIGH);
155delayMicroseconds(10);
156digitalWrite(trigPin1,
157 LOW);
158dur = pulseIn(echoPin1, HIGH);
159return (dur/58);
160 to centimeters.
161}
162
163long RightSensor ()
164{
165long dur;
166digitalWrite(trigPin2,
167 LOW);
168delayMicroseconds(2);
169digitalWrite(trigPin2,
170 HIGH);
171delayMicroseconds(10);
172digitalWrite(trigPin2,
173 LOW);
174dur = pulseIn(echoPin2, HIGH);
175return (dur/58);
176 to centimeters.
177}
178long LeftSensor ()
179{
180long dur;
181digitalWrite(trigPin3,
182 LOW);
183delayMicroseconds(2);
184digitalWrite(trigPin3,
185 HIGH);
186delayMicroseconds(10);
187digitalWrite(trigPin3,
188 LOW);
189dur = pulseIn(echoPin3, HIGH);
190return (dur/58);
191 to centimeters.
192}
inaki_lzcn
7 months ago
Hi, i'm kinda new to the maze solving robtos, i was wondering in which part of the code happens the mapping of the maze and the later resolution, thx