Arduino Bluetooth Controlled Multi-modal Robotic Car (Android and iOS Compatible)
Jump to navigation
Jump to search
Introduction
The Arduino micro-controller is capable of running only one sketch at a time. Therefore, it is generally thought that the Arduino can only perform one task or achieve one objective per-sketch.
This project shows the implementation of a multi-modal bluetooth controlled robotic car, capable of achieving different tasks and objectives, and is controlled with an Android or iOS mobile application. Everything happens in real-time, with little to no delay.
Specifications
The communication with the robot is made possible via Serial-over-Bluetooth protocol. A bluetooth module is connected to the Arduino compatible board, and the mobile phone which the application has been installed on can connect to the bluetooth module.
This robot has been built to have 5 modes:
- Line-follower Mode
- Object-follower Mode
- Light-follower Mode
- Ramble Mode
- Manual-control Mode
Hardware Components and Setup
- HM10 Bluetooth Module x 1
- Yourduino RoboRed Board x 1
- IR Edge Detection/Line Tracker Sensor x 1
- Photo-resistor x 1
- Buzzer x 1
- Ultrasonic sensor x 1
- L9110S Motor driver x 1
- LM393 Opto-interrupters x 2
- Geared DC motors x 2
- Micro servo motor kit x 1
- Robot car chassis with 2 wheels x 1
Configuring the HM10 BLE 4 Module
The HM10 module was chosen for this project due to it's compatibility with the common BLE 4.0 (Bluetooth Low Energy) protocol, used on Apple Iphones as well as Android phones. Before the module can be used, the baudrate, device name, and preferred pin have to be set. Below are steps for setting up the HM10 Module using the RoboRED board:
- The schematic of Arduino shows that the RX and TX pins are connected to the USB Interface chip (on Arduino board as pin 0 and pin 1), which means that those pins can directly access the USB Interface chip itself. We generally use TX and RX pins for communication. Upload the empty sketch below to the RoboRED board in order to enable it serve as USB-TTL converter for communicating with the HM10 module:
1 void setup() { 2 } 3 4 void loop() { 5 }
- Disconnect the Arduino USB connection
- Connect HM10-Tx --> RoboRED-Tx(Pin1) and HM10-Rx --> RoboRED-Rx(Pin0) (i.e. Tx --> Tx and Rx --> Rx of each board), then HM10-Vcc --> RoboRED-5V line (any of the pins with a red base) on the RoboRED and HM10-GND --> RoboRED-GND (any of the pins with a blue base)
- Plug in the RoboRED's USB to the PC and open the Arduino IDE. The HM-10 Red LED should start flashing (means it is not paired yet).
- Open the Serial Monitor and set the baudrate to 9600 at the lower right. In the box just to the left, set "Both NL & CR"
- NOTE: ALL commands you type to the HM-10 must be in UPPER CASE.
- Type AT and press enter. If the response is OK, then the module is ready to be configured using AT commands
- Type AT+VERR? command to check the firmware version that runs on your HM10 module. V5** and above is good
- Type AT+NAME? to see current name of the module, otherwise, type AT+NAMEpreferredName to set a preferred Bluetooth name for the module. Kindly note that there should be NO space between the AT+NAME and the preferredName chosen (e.g. AT+NAMERobotCar)
- Next, set the passcode that would be required for connecting to the module; AT+PASSpreferredPasscode (e.g.AT+PASS123456). The preferred passcode must be 6 numbers. To check the passcode, use AT+PASS?
- The HM10 module is configured to connect without passcode (i.e. AT+TYPE0). Set it to request a passcode on pairing by using this command: AT+TYPE2
The module is now configured and ready to be used. To save your settings, use AT+RESET in order to save and reset the module. The red LED will continue to flash.
(Setting up) About the Edge Detection/Line Tracker Sensor
This Sensor is really a module which has an IR receiver and transmitter. IR Sensors work by using a specific light sensor to detect a select light wavelength in the Infra-Red (IR) spectrum. By using an LED which produces light at the same wavelength as what the sensor is looking for, you can look at the intensity of the received reflected light. When an object is close to the sensor, the light from the LED bounces (reflects) off the object and into the light sensor. This results in a voltage being sensed on the output pin of the module. As long as the sensor receives the reflected IR light, it sends a HIGH from the output of the IR module. When the Robot gets to an edge where the IR module no longer receives the reflected IR light, it sends a LOW from the output of the module. This is the principle used for edge detection.
For line tracking, a beam of infrared light is transmitted from the IR transmitter of the module onto the object, then the receiver measures how much light is reflected back. A dark object bounces off less light than a bright object (i.e. dark objects absorb more light than bright objects). This is the fundamental principle behind a line tracking robot. Hence, in a line tracking mode, the IR sensor detects the difference between white (a surface) and black (a line). Since black reflects little or no light, the module sends a LOW signal off its output, and a HIGH output signal when it detects white. This is then used to adjust the speed of the motors to track a black line.
Setting up the Motor and Motor Driver
In order to get the desired functionality of the motors and motor drivers, it is necessary to know what pins of the motor drivers control what direction. This is a tricky, but easy process and the steps are outlined:
- Disconnect the USB cable from the RoboRED when changing any wires..
- First, start with one motor (Motor A). Connect pin A-1A of the L9110 motor driver (or IN1 of L293N motor driver) to MA_dir pin on the Arduino and pin A-1B of the L9110 motor driver (or IN2 of L293N motor driver) to MA_spd pin on the Arduino which we have defined in the code.
1 // Motor Control Pins Direction and PWM Speed 2 const byte MA_dir = 4; 3 const byte MB_dir = 7; 4 const byte MA_spd = 5; 5 const byte MB_spd = 6; 6 7 void setup() /**************************** SETUP: RUNS ONCE *******************/ 8 { 9 pinMode(MA_dir, OUTPUT); 10 pinMode(MB_dir, OUTPUT); 11 pinMode(MA_spd, OUTPUT); 12 pinMode(MB_spd, OUTPUT); 13 14 }//--(end setup )--- 15 16 void loop() /******************** LOOP: RUNS CONSTANTLY *******************/ 17 { 18 front(); // move the wheel in the forward direction 19 } 20 21 void front() 22 { 23 digitalWrite(MA_dir, HIGH); 24 digitalWrite(MB_dir, HIGH); 25 analogWrite(MA_spd, 0); 26 analogWrite(MB_spd, 80); 27 }
- If Motor A moves in the desired front direction (i.e. in a clockwise direction if the front of the motor wheel is facing you), then motor one is all set; else, interchange the red and black wires of the motor that go into Motor A slots of the motor driver.
- To configure Motor B, Connect pin B-1A of the L9110 motor driver (or IN3 of L293N motor driver) to MB_dir pin on the Arduino and pin B-1B of the L9110 motor driver (or IN4 of L293N motor driver) to MB_spd pin on the Arduino which we have defined in the code. If Motor B moves in the desired front direction (i.e. in an anti-clockwise direction if the front of the motor wheel is facing you), then motor one is all set; else, interchange the red and black wires of the motor that go into Motor B slots of the motor driver.
After confirming the front direction (which also confirms the back direction), it is important to also test the left direction (which would also confirm the right direction).
Upload this code to the Arduino. To turn the robot in the left direction, the left motor should rotate backwards (i.e. in a clockwise direction if the front of the motor wheel is facing you) while the right motor rotates forward (also in a clockwise direction if the front of the motor wheel is facing you). This is known as Differential Steering.
1 const byte MA_dir = 4; // Motor Control Pins Direction and PWM Speed
2 const byte MB_dir = 7;
3 const byte MA_spd = 5;
4 const byte MB_spd = 6;
5
6 void setup() /**************************** SETUP: RUNS ONCE *******************/
7 {
8 pinMode(MA_dir, OUTPUT);
9 pinMode(MB_dir, OUTPUT);
10 pinMode(MA_spd, OUTPUT);
11 pinMode(MB_spd, OUTPUT);
12
13 }//--(end setup )---
14
15 void loop() /******************** LOOP: RUNS CONSTANTLY *******************/
16 {
17 left(); // move the wheel in the forward direction
18 }
19
20 void left()
21 {
22 digitalWrite(MA_dir, LOW);
23 digitalWrite(MB_dir, HIGH);
24 analogWrite(MA_spd, 80);
25 analogWrite(MB_spd, 80);
26 }
If the Robot turns right after running this sketch in the Arduino, interchange the pins on motor A with the pins of motor B, exactly they way they are, i.e. MA_dir should now be MB_dir and vice versa, likewise MA_spd should now be MB_spd and vice versa.
NOTE: The speed of the motor increases as the PWM (analogWrite) value reduces. The range is from 0 - 255 (fastest to slowest speed)
Setting up the Servo
It is necessary to determine the actual angle of rotation of the servo motor each direction (left, right, and front) represents.
Upload the code below to the Arduino and change the servo.write(<angle>) angle till the desired values for left, right and front positions of the servo which controls the ultrasonic sensor is achieved. The datatype of the variable should be a byte.
1 #include <Servo.h> //Import the Servo library
2
3 Servo servo; //create servo object to control a servo, max = 8 servos
4 byte servoPin = 11;
5
6 void setup() /**************************** SETUP: RUNS ONCE *******************/
7 {
8 pinMode(servoPin, OUTPUT);
9 servo.attach(servoPin);
10 servo.write(95); //change this value till you achieve the desired value for left, front, and right directions
11
12 }//--(end setup )---
13
14 void loop() /******************** LOOP: RUNS CONSTANTLY *******************/
15 {
16
17 }
Testing the Ultrasonic Range Sensor
To test if the ultrasonic range sensor has no faults, upload the code below into the Arduino and open the Serial Monitor at baudrate 9600 to see if you get correct values. Try to place an object in front of the sensor and vary the distance to see if there is a corresponding change in the values displayed on the Serial Monitor.
1 /*-----( Import needed library )-----*/
2 #include <SimpleKalmanFilter.h>
3
4
5 /*-----( Declare Constants and Pin Numbers )-----*/
6 //ultrasonic sensor parameters
7 const byte trigPin = 9;
8 const byte echoPin = 8;
9
10 SimpleKalmanFilter simpleKalmanFilter(2, 2, 0.01); //create an object for the simpleKalmanFilter library
11
12 float dist = 0.00;
13 float estimated_value = 0.00;
14
15
16 void setup() /**************************** SETUP: RUNS ONCE *******************/
17 {
18 pinMode(trigPin, OUTPUT);
19 pinMode(echoPin, INPUT);
20 Serial.begin(9600);
21
22 }//--(end setup )---
23
24
25 void loop() /******************** LOOP: RUNS CONSTANTLY *******************/
26 {
27 Serial.println(obstacle());
28 }
29
30
31 float obstacle()
32 {
33 digitalWrite(trigPin, LOW);
34 delayMicroseconds(2);
35 digitalWrite(trigPin, HIGH);
36 delayMicroseconds(12);
37 digitalWrite(trigPin, LOW);
38 dist = pulseIn(echoPin, HIGH);
39 delayMicroseconds(10000);
40 dist = dist / 58;
41
42 //smoothing of ultrasonic sensor readings
43 estimated_value = simpleKalmanFilter.updateEstimate(dist);
44
45 return estimated_value;
46
47 }//END obstacle
Testing the Light Dependent Resistor (LDR)
This sketch is use to confirm that the ultrasonic range sensor is working fine. Use a torch light and vary its distance from the LDR to see the different readings on the Serial Monitor. This can also be used to set a desired lightThreshold in the main code.
1 const byte ldrPin = A4;
2
3 void setup() /**************************** SETUP: RUNS ONCE *******************/
4 {
5 pinMode(ldrPin, INPUT);
6
7 Serial.begin(9600);
8
9 }//--(end setup )---
10
11 void loop() /******************** LOOP: RUNS CONSTANTLY *******************/
12 {
13 // display light intensity values on the Serial Monitor
14 Serial.println(analogRead(ldrPin));
15 }
Using the Mobile Application
In order to control the robot car, a Mobile App has been designed with bluetooth capabilities to be able able to interface with the car via its HM10 BLE 4.0 module. BLE 4.0 protocol was chosen as it is compatible with both Android and iOS devices, and it consumes low energy. The Mobile Application has been written in two versions: one for Ios (Apple) devices and one for Android devices.
The Ios App will be Installable from a 'Store'.
The Android App can be downloaded HERE:
When this file is Unzipped it will produce a .apk file
Code and Mobile Application
1 /* YourDuino / KUFY EBONG Robot Control
2 - WHAT IT DOES Controls a Keyes robot in multiple modes
3 - SEE the comments after "//" on each line below
4
5 - V1.01 16 August 2019
6 - V1.01 18 September 2019: corrected autoDrive() function, added servo for ultrasonic, added lightFollower() function, added edge detection
7 Questions: kuffykeys@gmail.com , terry@yourduino.com */
8
9 /*-----( Import needed libraries )-----*/
10 #include <SimpleKalmanFilter.h>
11 #include <Servo.h>
12
13
14 /*-----( Declare Constants and Pin Numbers )-----*/
15 //ultrasonic sensor parameters PINS USED: 0,1,2,3,4,5,6,7,8,9,10,12,13,A0,A1,A2,A3,A4 remainder---->11,A5 for the 2 other photo sensors.... Not enough pins on UNO. That means we have to use just 1 photo sensor in front
16 const byte trigPin = 9;
17 const byte echoPin = 8;
18
19 const byte ldrPin = A4;
20
21 byte servoPin = 11;
22
23 byte thresholdDist = 35;
24
25 int buzzInterval = 600;
26
27 int lightThreshold = 600;
28
29 /*************positions of servo *****************/
30 byte lookLeft = 175, lookRight = 30, lookAhead = 95;
31
32 /***********flags for the millis state ********************/
33 bool state1 = false, state2 = false, state3 = false, state4 = false, state5 = false, state6 = false;
34
35
36 bool resetBuzz = true, reset1 = true, reset2 = true, reset3 = true, reset4 = true, reset5 = true, reset6 = true;
37
38 float dist1 = NULL;
39
40
41 /*************maybe create flags for the state of the robots movement declare these variables when the autoDrive state is entered*******************/
42
43 unsigned long prevMillis1 = 0l, prevMillis2 = 0l, prevMillis3 = 0l, prevMillis4 = 0l, prevMillis5 = 0l, prevMillis6 = 0l, currentMillis = 0l;
44 unsigned long prevBuzzMillis = 0l, currentBuzzMillis = 0l;
45
46
47 int pauseInterval = 1000;
48
49
50 const byte leftLine = A1; // PhotoSensors in Line Follower
51 const byte middleLine = A2;
52 const byte rightLine = A3;
53
54 const byte MA_dir = 4; // Motor Control Pins Direction and PWM Speed
55 const byte MB_dir = 7;
56 const byte MA_spd = 5;
57 const byte MB_spd = 6;
58
59 const byte buzzerPin = 13; //meant for obstacle alert
60
61
62 /*-----( Declare objects )-----*/
63 SimpleKalmanFilter simpleKalmanFilter(2, 2, 0.01);
64 Servo servo; //create servo object to control a servo, max = 8 servos
65
66 /*-----( Declare Variables )-----*/
67
68 float dist = 0.00;
69 float obstacleThreshold = 35; //Threshold distance where the robot sees object in front of it as an obstacle
70 float estimated_value = 0;
71
72
73 /*------( flags to hold the different robot modes )-----*/
74
75 bool manMode = false;
76 bool lineMode = false;
77 bool autoMode = false;
78 bool followMode = false;
79 bool StopMode = false;
80 bool frontMode = false;
81 bool backMode = false;
82 bool leftMode = false;
83 bool rightMode = false;
84 bool lightMode = false;
85
86 /*-----( variable to hold data from Serial )-----*/
87
88 char val = ' ';
89 char val1 = ' ';
90
91
92
93 void setup() /**************************** SETUP: RUNS ONCE *******************/
94 {
95 pinMode(leftLine, INPUT);
96 pinMode(middleLine, INPUT);
97 pinMode(rightLine, INPUT);
98 pinMode(ldrPin, INPUT);
99 pinMode(buzzerPin, OUTPUT);
100 pinMode(servoPin, OUTPUT);
101 pinMode(trigPin, OUTPUT);
102 pinMode(echoPin, INPUT);
103 pinMode(MA_dir, OUTPUT);
104 pinMode(MB_dir, OUTPUT);
105 pinMode(MA_spd, OUTPUT);
106 pinMode(MB_spd, OUTPUT);
107
108
109 servo.attach(servoPin);
110 servo.write(95); //default central position
111
112 Serial.begin(9600);
113
114 }//--(end setup )---
115
116 void loop() /******************** LOOP: RUNS CONSTANTLY *******************/
117 {
118 // buzzer();
119 // delay(10);
120 // buzzer();
121 // delay(500);
122 // Serial.println(detectLight());
123 autoDrive();
124 if (Serial.available())
125 {
126 val = Serial.read();
127 switch (val) {
128
129 case 'M':
130 manMode = true;
131 lineMode = false;
132 autoMode = false;
133 followMode = false;
134 lightMode = false;
135 servo.write(lookAhead);
136 break;
137
138 case 'f':
139 manMode = false;
140 lineMode = false;
141 autoMode = false;
142 followMode = true;
143 lightMode = false;
144 servo.write(lookAhead);
145 break;
146
147 case 'T':
148 manMode = false;
149 lineMode = true;
150 autoMode = false;
151 followMode = false;
152 lightMode = false;
153 servo.write(lookAhead);
154 break;
155
156 case 't':
157 manMode = false;
158 lineMode = false;
159 autoMode = false;
160 followMode = false;
161 lightMode = true;
162 servo.write(lookAhead);
163 break;
164
165 case 'A':
166 manMode = false;
167 lineMode = false;
168 autoMode = true;
169 followMode = false;
170 lightMode = false;
171
172 dist1 = obstacle();
173 state1 = true;
174 state2 = false; state3 = false; state4 = false; state5 = false; state6 = false;
175 reset1 = true; reset2 = true; reset3 = true; reset4 = true; reset5 = true; reset6 = true;
176
177 case 'S':
178 StopMode = true;
179 frontMode = false;
180 backMode = false;
181 leftMode = false;
182 rightMode = false;
183 lightMode = false;
184 break;
185
186 case 'F':
187 StopMode = false;
188 frontMode = true;
189 backMode = false;
190 leftMode = false;
191 rightMode = false;
192 lightMode = false;
193 break;
194
195 case 'B':
196 StopMode = false;
197 frontMode = false;
198 backMode = true;
199 leftMode = false;
200 rightMode = false;
201 lightMode = false;
202 break;
203
204 case 'L':
205 StopMode = false;
206 frontMode = false;
207 backMode = false;
208 leftMode = true;
209 rightMode = false;
210 lightMode = false;
211 break;
212
213 case 'R':
214 StopMode = false;
215 frontMode = false;
216 backMode = false;
217 leftMode = false;
218 rightMode = true;
219 lightMode = false;
220 break;
221
222 default:
223 StopMode = true;
224 frontMode = false;
225 backMode = false;
226 leftMode = false;
227 rightMode = false;
228 lightMode = false;
229 servo.write(lookAhead);
230 break;
231 }
232 }// END If Serial Vaialable
233
234 if (manMode)
235 {
236 if (StopMode) {
237 Stop();
238 }
239
240 if (frontMode) {
241 front();
242 }
243
244 if (backMode) {
245 back();
246 }
247
248 if (leftMode) {
249 left();
250 }
251
252 if (rightMode) {
253 right();
254 }
255 }// END If ManMode
256
257 if (lineMode)
258 {
259 trackLine();
260 }
261
262 if (autoMode) {
263 autoDrive();
264 }
265
266 if (followMode) {
267 follower();
268 }
269
270 if (lightMode) {
271 detectLight();
272 }
273
274 }//--(end main loop )---
275
276
277
278 /*-----( Declare User-written Functions )-----*/
279 void front()
280 {
281
282 if (digitalRead(middleLine) == HIGH)
283 {
284 back();
285 }
286
287 if (digitalRead(middleLine) == LOW)
288 {
289 digitalWrite(MA_dir, HIGH);
290 digitalWrite(MB_dir, HIGH);
291 analogWrite(MA_spd, 0);
292 analogWrite(MB_spd, 0);
293 }
294 }
295
296 void back()
297 {
298 digitalWrite(MA_dir, LOW);
299 digitalWrite(MB_dir, LOW);
300 analogWrite(MA_spd, 0);
301 analogWrite(MB_spd, 0);
302 }
303
304 void Stop()
305 {
306 digitalWrite(MA_dir, HIGH);
307 digitalWrite(MB_dir, HIGH);
308 analogWrite(MA_spd, 255);
309 analogWrite(MB_spd, 255);
310 }
311
312 void left()
313 {
314 digitalWrite(MA_dir, LOW);
315 digitalWrite(MB_dir, HIGH);
316 analogWrite(MA_spd, 80);
317 analogWrite(MB_spd, 80);
318 }
319
320 void right()
321 {
322 digitalWrite(MA_dir, HIGH);
323 digitalWrite(MB_dir, LOW);
324 analogWrite(MA_spd, 80);
325 analogWrite(MB_spd, 80);
326 }
327
328 void trackLine()
329 {
330 if (digitalRead(middleLine))
331 {
332 if (digitalRead(rightLine) && !(digitalRead(leftLine)))
333 {
334 front();
335 }
336 else if (!(digitalRead(rightLine)) && digitalRead(leftLine))
337 {
338 front();
339 }
340 else
341 {
342 front();
343 }
344 }
345 else
346 {
347 if (digitalRead(rightLine) && !(digitalRead(leftLine)))
348 {
349 left();
350 }
351 else if (!(digitalRead(rightLine)) && digitalRead(leftLine))
352 {
353 right();
354 }
355 else
356 {
357 Stop();
358 }
359 }
360 }// END TrackLine
361
362 float obstacle()
363 {
364 digitalWrite(trigPin, LOW);
365 delayMicroseconds(2);
366 digitalWrite(trigPin, HIGH);
367 delayMicroseconds(12);
368 digitalWrite(trigPin, LOW);
369 dist = pulseIn(echoPin, HIGH);
370 delayMicroseconds(10000);
371 dist = dist / 58;
372
373 //come up with a filtering, smoothening algorithm here for ultrasonic
374 estimated_value = simpleKalmanFilter.updateEstimate(dist);
375
376 // Serial.println(estimated_value);
377 return estimated_value;
378
379 }//END obstacle
380
381 void autoDrive()
382 {
383 if (state1 == true)
384 {
385 servo.write(lookAhead);
386 front();
387
388 dist1 = obstacle();
389 state2 = false; state3 = false; state4 = false; state5 = false; state6 = false;
390
391 //for debugging
392 Serial.println("looking ahead + moving forward + read dist1_______state1");
393 }
394
395
396 if (dist1 < thresholdDist && !state3 && !state4)
397 {
398 state1 = false; state2 = true; state3 = false; state4 = false; state5 = false; state6 = false;
399
400 if (state2 == true && !state3 && !state4)
401 {
402 Stop();
403 servo.write(lookLeft);
404 unsigned long currentMillis2 = millis();
405 reset2 = false;
406 reset1 = true; reset3 = true; reset4 = true; reset5 = true; reset6 = true;
407 float dist2 = 0.0;;
408
409
410
411 //for debugging
412 Serial.println ("Stop + lookLeft___________state2");
413
414 if (currentMillis2 - prevMillis2 >= pauseInterval)
415 {
416 dist2 = obstacle();
417 state2 = false;
418 reset2 = true;
419
420 //for debugging
421 Serial.println("read left distance + leave state 2-----------------------------------------------------(dist2)" + String(dist2));
422
423 if (dist2 > thresholdDist && !state1)
424 {
425 state1 = false; state2 = false; state3 = true; state4 = false; state5 = false; state6 = false;
426 }
427
428 if (dist2 < thresholdDist && !state5)
429 {
430 state1 = false; state2 = false; state3 = false; state4 = true; state5 = false; state6 = false;
431 }
432 }
433 }
434 }
435
436
437 if (state3 == true)
438 {
439 servo.write(lookAhead);
440 left();
441 unsigned long currentMillis3 = millis();
442 reset3 = false;
443 reset1 = true; reset2 = true; reset4 = true; reset5 = true; reset6 = true;
444
445 //for debugging
446 Serial.println("lookAhead + move left __________state3");
447
448 if (currentMillis3 - prevMillis3 >= pauseInterval)
449 {
450 state1 = true;
451 state2 = false; state3 = false; state4 = false; state5 = false; state6 = false;
452 reset1 = false; reset2 = true; reset3 = true; reset4 = true; reset5 = true; reset6 = true;
453
454 //for debugging
455 Serial.println("leave state3 and proceed to state 1");
456 }
457 }
458
459 if (state4 == true)
460 {
461 Stop();
462 servo.write(lookRight);
463 unsigned long currentMillis4 = millis();
464 reset4 = false;
465
466 float dist3 = 0.0;
467
468 //for debugging
469 Serial.println("Stop + lookRight __________state4");
470
471 if (currentMillis4 - prevMillis4 >= pauseInterval)
472 {
473 dist3 = obstacle();
474 state4 = false;
475 reset4 = true;
476
477 //for debugging
478 Serial.println("read right distance and left state 4-----------------------------------------------------(dist3)" + String(dist3));
479
480 if (dist3 > thresholdDist && !state1)
481 {
482 state1 = false; state2 = false; state3 = false; state4 = false; state5 = true; state6 = false;
483 }
484
485 if (dist3 < thresholdDist && !state1)
486 {
487 state1 = false; state2 = false; state3 = false; state4 = false; state5 = false; state6 = true;
488 }
489 }
490 }
491
492
493 if (state5 == true)
494 {
495 servo.write(lookAhead);
496 right();
497 unsigned long currentMillis5 = millis();
498 reset5 = false;
499
500 //for debugging
501 Serial.println("lookAhead + move right____________________state5");
502
503 if (currentMillis - prevMillis5 >= pauseInterval)
504 {
505 state5 = false;
506 reset5 = true;
507 state1 = true;
508
509 //for debugging
510 Serial.println("left state 5 to state 1");
511 }
512 }
513
514
515 if (state6 == true)
516 {
517 back();
518 servo.write(lookAhead);
519 unsigned long currentMillis6 = millis();
520 reset6 = false;
521
522 //for debugging
523 Serial.println("moveBackward + lookAhead ___________________state6");
524
525 if (currentMillis6 - prevMillis6 >= pauseInterval)
526 {
527 state6 = false;
528 reset6 = true;
529 state2 = true;
530
531 //for debugging
532 Serial.println("left state 6 to state 2 to scan again and see where to go");
533 }
534 }
535
536
537
538 if (reset1)
539 prevMillis1 = millis();
540
541 if (reset2)
542 prevMillis2 = millis();
543
544 if (reset3)
545 prevMillis3 = millis();
546
547 if (reset4)
548 prevMillis4 = millis();
549
550 if (reset5)
551 prevMillis5 = millis();
552
553 if (reset6)
554 prevMillis6 = millis();
555
556
557
558
559 else
560 {
561 state1 = true; state2 = false; state3 = false; state4 = false; state5 = false; state6 = false;
562 dist1 = obstacle();
563 Serial.println("the else statement has happened");
564 }
565
566
567 //for debugging
568 Serial.println("nothing happening here-----------------------------------------------------(dist1)" + String(dist1));
569 }//END Autodrive
570
571
572 void detectLight()
573 {
574 int lightIntensity = analogRead(ldrPin);
575 currentBuzzMillis = millis();
576
577 if (resetBuzz)
578 {
579 prevBuzzMillis = millis();
580 }
581
582
583 if (lightIntensity > lightThreshold)
584 {
585 front();
586 resetBuzz = false;
587
588 if (currentBuzzMillis - prevBuzzMillis >= buzzInterval)
589 {
590 digitalWrite (buzzerPin, LOW);
591 resetBuzz = true;
592
593 //for debugging
594 Serial.println("resetBUZZZZZ_________________________________________________________________________");
595 }
596
597 else
598 {
599 digitalWrite (buzzerPin, HIGH);
600 }
601
602 //for debugging
603 Serial.println("detected light ahead_____________________________________________________moving forward");
604 }
605
606 else {
607 Stop();
608 digitalWrite (buzzerPin, LOW);
609
610 //for debugging
611 Serial.println("Stopped because light is not up to threshold");
612 }
613 }
614
615
616 void follower()
617 {
618 dist = obstacle();
619
620 //come up with a filtering, smoothening algorithm here for ultrasonic
621 estimated_value = simpleKalmanFilter.updateEstimate(dist);
622
623 if (estimated_value < 100 && estimated_value > 30) {
624 //This condition is to move forward the robot when the object is in the range of 100 to 30 centimeters.
625 front();
626 }
627
628 if (estimated_value < 30 && estimated_value > 20) {
629 //This condition is to make the robot stable when the object is in the range of 20 to 30 centimeters.
630 Stop();
631 }
632
633 if (estimated_value < 20 && estimated_value > 2) {
634 //This condition is to move backward the robot when the object is in the range of 2 to 20 centimeters.
635 back();
636 }
637
638 if (estimated_value > 100) {
639 //This condition is to stop the robot when the object is in the out of range i.e greater than 100 centimeters.
640 Stop();
641 }
642 }// END Follower
643
644 //*********( THE END )***********