1
33
34#include <Servo.h>
35
36
37 ****Calibrate servos' center angle (in microseconds because we'll use "xx.writeMicroseconds();"
38 command). Start with 1500.
39const int ANGLE_mid_Shaft = 1520;
40const int ANGLE_mid_FLeft
41 = 1550;
42const int ANGLE_mid_FRight = 1570;
43const int ANGLE_mid_BLeft = 1450;
44const
45 int ANGLE_mid_BRight = 1450;
46
47const int ANGLE_sweep = 250;
48 this value (in microseconds) to determine how wide the servos will sweep (legs'
49 step width). Bigger value means wider sweep angle.
50const int ANGLE_res = 10;
51
52 minimum servo's default dead band width (highest resolution) or more (less resolution).
53 Example: SG90 servo dead band width is 10 microseconds.
54int sweepSPEED;
55 variable to determine how fast the servos will sweep.
56int sweepSPEED_Rand[3]
57 = {4, 6, 8};
58 the speed (in miliseconds) for each mode. Smaller value means faster.
59
60const
61 int ANGLE_turnMAX = ANGLE_sweep * 1.5;
62 much maximum the bot will turn toward light. Bigger value means bigger turn.
63const
64 int ANGLE_turnNARROW = ANGLE_sweep * 0.25;
65 much maximum the bot will turn avoiding objects at its sides in narrow space. Bigger
66 value means bigger turn.
67
68const int SONAR_sum = 3;
69const
70 int PHOTO_sum = 4;
71int PIN_trig[SONAR_sum] = {13,
72 11, 8};
73 {front, left, right}.
74int PIN_ec[SONAR_sum] = {12, 10, 7};
75 pins connected to ultrasonic sensors' echo pins; {front, left, right}.
76int PIN_PHOTO[PHOTO_sum]
77 = {2, 3, 1, 0};
78 left, front right, back left, back right}.
79
80const int distRotate = 25;
81 **Configure minimum distance (in cm) between the robot and obstacle before the robot
82 avoid it by rotating.
83const int distRetreat = 10;
84 distance (in cm) between the robot and obstacle before the robot avoid it by retreating.
85const
86 int distTurn = 20;
87 and obstacle before the robot avoid it by turning.
88const int counter_gait_max
89 = 8;
90 rotating or retreating).
91
92
93const
94 int RUN_time = 25000;
95const int REST_time = 3000;
96
97
98int
99 SONAR_id;
100const int FRONT = 0;
101const int LEFT = 1;
102const int RIGHT = 2;
103
104
105 ID's for photodiodes:
106const int FRONT_LEFT = 0;
107const int FRONT_RIGHT = 1;
108const
109 int BACK_LEFT = 2;
110const int BACK_RIGHT = 3;
111
112
113 reading:
114int PHOTO_Front_Left;
115int PHOTO_Front_Right;
116int PHOTO_Back_Left;
117int
118 PHOTO_Back_Right;
119
120const int SONAR_TrigSig = 10;
121 S) of trigger signal the sensors needed to produce ultrasonic sound (already specified
122 by the products, don't change this value).
123const unsigned long SONAR_MaxEc =
124 50000;
125 specified by the products, don't change this value).
126const float SOUND_speed
127 = 0.034;
128 avatar Aang is needed to do air bending if this value is wanted to be changed).
129int
130 distance[SONAR_sum];
131
132
133 Ddeclaration of servos:
134Servo SERVO_shaft;
135Servo SERVO_front_left;
136Servo
137 SERVO_front_right;
138Servo SERVO_back_left;
139Servo SERVO_back_right;
140
141
142 Variables for each servos' angles:
143int ANGLE_shaft = ANGLE_mid_Shaft;
144int
145 ANGLE_front_left = ANGLE_mid_FLeft;
146int ANGLE_front_right = ANGLE_mid_FRight;
147int
148 ANGLE_back_left = ANGLE_mid_BLeft;
149int ANGLE_back_right = ANGLE_mid_BRight;
150
151
152 Angle manipulation for middle servo (shaft).
153const int ANGLE_max_Shaft = ANGLE_mid_Shaft
154 + ANGLE_sweep;
155const int ANGLE_min_Shaft = ANGLE_mid_Shaft - ANGLE_sweep;
156int
157 ANGLE_sweep_val;
158
159
160int
161 ANGLE_shaft_record;
162int ANGLE_front_left_record;
163int ANGLE_front_right_record;
164int
165 ANGLE_back_left_record;
166int ANGLE_back_right_record;
167
168
169 servos angles correction according to light detection:
170int LIGHT_left;
171int
172 LIGHT_right;
173
174int
175 SONAR_left;
176int SONAR_right;
177
178
179 that I'm always not sure how to explain. :(
180int ANGLE_prev;
181int flag_shaft_reverse;
182int
183 flag_transition_rotate;
184int flag_transition_start = 1;
185int flag_rest = 0;
186int
187 flag_RUN_time = 0;
188int rotate_random;
189int counter_gait;
190
191void setup()
192 {
193
194
195
196 SERVO_shaft.attach(2);
197 on arduino.
198 SERVO_front_left.attach(4);
199 pin on arduino.
200 SERVO_front_right.attach(3);
201 signal pin on arduino.
202 SERVO_back_left.attach(6);
203 servo's signal pin on arduino.
204 SERVO_back_right.attach(5);
205 servo's signal pin on arduino.
206
207
208 angles.
209 SERVO_shaft.writeMicroseconds(ANGLE_mid_Shaft);
210 SERVO_front_left.writeMicroseconds(ANGLE_mid_FLeft);
211
212 SERVO_front_right.writeMicroseconds(ANGLE_mid_FRight);
213 SERVO_back_left.writeMicroseconds(ANGLE_mid_BLeft);
214
215 SERVO_back_right.writeMicroseconds(ANGLE_mid_BRight);
216
217
218 for sonars, both pinMode and value.
219 for(SONAR_id = 0; SONAR_id < SONAR_sum;
220 SONAR_id++) {
221 pinMode(PIN_trig[SONAR_id],OUTPUT);
222 pinMode(PIN_ec[SONAR_id],INPUT);
223
224 digitalWrite(PIN_trig[SONAR_id], LOW);
225 }
226
227 randomSeed(analogRead(5));
228
229 SONAR_READ_ALL();
230
231 SONAR_READ_ALL();
232
233 But believe me, it's needed.
234 START();
235 sure which legs are lifted randomly at the first step.
236
237 delay(3000);
238 **dum.. daa.. dum..
239}
240
241void loop() {
242 int state = random(0,2);
243
244
245 if(state == 0) {
246 REST();
247 }
248 else {
249 int randomSPEED
250 = random(0,3);
251 sweepSPEED = sweepSPEED_Rand[randomSPEED];
252 RUN();
253
254 flag_rest = 0;
255 }
256}
257
258void REST() {
259 if(flag_rest == 0) {
260
261 TRANSITION_GAIT();
262 flag_rest = 1;
263 }
264 delay(REST_time);
265}
266
267void
268 RUN() {
269 unsigned long TIMER_state = millis();
270 while((millis() - TIMER_state)
271 <= RUN_time) {
272 if(distance[FRONT] > distRotate) {
273 flag_RUN_time
274 = 0;
275 while(flag_RUN_time == 0) {
276 FORWARD();
277 }
278 }
279
280
281 while(distance[FRONT] > distRetreat && distance[FRONT] <= distRotate)
282 {
283 while(distance[LEFT] > distance[RIGHT]) {
284 ROTATE_LEFT_AVOID();
285
286 break;
287 }
288 while(distance[LEFT] < distance[RIGHT]) {
289
290 ROTATE_RIGHT_AVOID();
291 break;
292 }
293 while(distance[LEFT]
294 == distance[RIGHT]) {
295 ROTATE_RANDOM_AVOID();
296 break;
297 }
298
299 }
300
301 while(distance[FRONT] <= distRetreat) {
302 RETREAT_AVOID();
303
304 }
305 }
306}
307
308
311
312
314
315void SHAFT() {
316 unsigned
317 long TIMER_servo = millis();
318 while((millis() - TIMER_servo) <= sweepSPEED)
319 {
320 while(ANGLE_shaft == ANGLE_mid_Shaft) {
321 counter_gait++;
322 SONAR_READ_ALL();
323
324 LIGHT_COMPARE_EXECUTE();
325 SIDE_AVOID();
326 flag_RUN_time = 1;
327
328 break;
329 }
330 }
331
332 if(ANGLE_prev < ANGLE_shaft && ANGLE_shaft
333 < ANGLE_max_Shaft) {
334 ANGLE_prev = ANGLE_shaft;
335 ANGLE_shaft += ANGLE_res;
336
337 }
338 else if(ANGLE_shaft >= ANGLE_max_Shaft) {
339 ANGLE_prev = ANGLE_shaft;
340
341 ANGLE_shaft -= ANGLE_res;
342 }
343 else if(ANGLE_prev > ANGLE_shaft && ANGLE_shaft
344 > ANGLE_min_Shaft) {
345 ANGLE_prev = ANGLE_shaft;
346 ANGLE_shaft -= ANGLE_res;
347
348 }
349 else if(ANGLE_shaft <= ANGLE_min_Shaft) {
350 ANGLE_prev = ANGLE_shaft;
351
352 ANGLE_shaft += ANGLE_res;
353 }
354 SERVO_shaft.writeMicroseconds(ANGLE_shaft);
355}
356
357void
358 SHAFT_REVERSE() {
359 if(ANGLE_prev < ANGLE_shaft) {
360 ANGLE_prev = ANGLE_shaft
361 + 1;
362 }
363 else if(ANGLE_prev > ANGLE_shaft) {
364 ANGLE_prev = ANGLE_shaft
365 - 1;
366 }
367}
368
369
371
372
373
375
376void TRANSITION_GAIT()
377 {
378 ANGLE_front_left_record = ANGLE_front_left;
379 ANGLE_front_right_record
380 = ANGLE_front_right;
381 ANGLE_back_left_record = ANGLE_back_left;
382 ANGLE_back_right_record
383 = ANGLE_back_right;
384 ANGLE_shaft_record = ANGLE_shaft;
385
386 int flag =
387 HIGH;
388 int counter = 0;
389 while(flag == HIGH) {
390 SHAFT();
391 LIGHT_left
392 = 0;
393 LIGHT_right = 0;
394
395 counter++;
396
397 ANGLE_front_left
398 = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_front_left_record, ANGLE_mid_FLeft);
399
400 ANGLE_front_right = map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_front_right_record,
401 ANGLE_mid_FRight);
402 ANGLE_back_left = map(counter, 1, ((ANGLE_sweep * 2) /
403 ANGLE_res), ANGLE_back_left_record, ANGLE_mid_BLeft);
404 ANGLE_back_right =
405 map(counter, 1, ((ANGLE_sweep * 2) / ANGLE_res), ANGLE_back_right_record, ANGLE_mid_BRight);
406
407
408 SERVO_shaft.writeMicroseconds(ANGLE_shaft);
409 SERVO_front_left.writeMicroseconds(ANGLE_front_left);
410
411 SERVO_front_right.writeMicroseconds(ANGLE_front_right);
412 SERVO_back_left.writeMicroseconds(ANGLE_back_left);
413
414 SERVO_back_right.writeMicroseconds(ANGLE_back_right);
415
416 if(counter
417 == ((ANGLE_sweep * 2) / ANGLE_res)) {
418 flag = LOW;
419 START();
420
421 flag_transition_rotate = 0;
422 }
423 }
424}
425
426void TRANSITION_START()
427 {
428 if(ANGLE_shaft == ANGLE_mid_Shaft || (ANGLE_shaft > ANGLE_mid_Shaft && ANGLE_shaft
429 > ANGLE_prev) || (ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_shaft < ANGLE_prev)) {
430
431 ANGLE_sweep_val = 0;
432 }
433 else {
434 flag_transition_start = 0;
435
436 }
437}
438
439void START() {
440 ANGLE_prev = random((ANGLE_shaft), (ANGLE_shaft
441 + 2));
442 if(ANGLE_prev == ANGLE_shaft) {
443 ANGLE_prev -= 1;
444 }
445 flag_transition_start
446 = 1;
447}
448
449int ROTATE_RANDOM() {
450 return random(0, 2);
451}
452
453
455
456
457
459
460void FORWARD() {
461 while(flag_transition_rotate
462 == 2) {
463 TRANSITION_GAIT();
464 }
465 flag_transition_rotate = 1;
466
467
468 while(flag_shaft_reverse == 0) {
469 SHAFT_REVERSE();
470 break;
471 }
472
473 flag_shaft_reverse = 1;
474
475 while(flag_transition_start == 1) {
476 SHAFT();
477
478 TRANSITION_START();
479 WALK();
480 }
481
482 SHAFT();
483 ANGLE_sweep_val
484 = ANGLE_sweep;
485 WALK();
486}
487
488void RETREAT() {
489 while(flag_transition_rotate
490 == 2) {
491 TRANSITION_GAIT();
492 }
493 flag_transition_rotate = 1;
494
495
496 while(flag_shaft_reverse == 1) {
497 SHAFT_REVERSE();
498 break;
499 }
500
501 flag_shaft_reverse = 0;
502
503 while(flag_transition_start == 1) {
504 SHAFT();
505
506 LIGHT_left = 0;
507 LIGHT_right = 0;
508 TRANSITION_START();
509 WALK();
510
511 }
512
513 SHAFT();
514 LIGHT_left = 0;
515 LIGHT_right = 0;
516 ANGLE_sweep_val
517 = (ANGLE_sweep * -1);
518 WALK();
519}
520
521void WALK() {
522 if(ANGLE_shaft
523 >= ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) {
524 ANGLE_front_left = map(ANGLE_shaft,
525 ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_FLeft - ANGLE_sweep_val) + LIGHT_left
526 + SONAR_left), ANGLE_mid_FLeft);
527 ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft,
528 ANGLE_max_Shaft, ((ANGLE_mid_FRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right),
529 ANGLE_mid_FRight);
530 ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft,
531 ((ANGLE_mid_BLeft + ANGLE_sweep_val) - LIGHT_left - SONAR_left), ANGLE_mid_BLeft);
532
533 ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, ((ANGLE_mid_BRight
534 + ANGLE_sweep_val) + LIGHT_right + SONAR_right), ANGLE_mid_BRight);
535 }
536 else
537 if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) {
538 ANGLE_front_left
539 = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, ((ANGLE_mid_FLeft
540 + ANGLE_sweep_val) - LIGHT_left - SONAR_left));
541 ANGLE_front_right = map(ANGLE_shaft,
542 ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, ((ANGLE_mid_FRight + ANGLE_sweep_val)
543 + LIGHT_right + SONAR_right));
544 ANGLE_back_left = map(ANGLE_shaft, ANGLE_max_Shaft,
545 ANGLE_mid_Shaft, ANGLE_mid_BLeft, ((ANGLE_mid_BLeft - ANGLE_sweep_val) + LIGHT_left
546 + SONAR_left));
547 ANGLE_back_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft,
548 ANGLE_mid_BRight, ((ANGLE_mid_BRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right));
549
550 }
551 else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) {
552
553 ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_FLeft
554 + ANGLE_sweep_val) - LIGHT_left - SONAR_left), ANGLE_mid_FLeft);
555 ANGLE_front_right
556 = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_FRight + ANGLE_sweep_val)
557 + LIGHT_right + SONAR_right), ANGLE_mid_FRight);
558 ANGLE_back_left = map(ANGLE_shaft,
559 ANGLE_mid_Shaft, ANGLE_min_Shaft, ((ANGLE_mid_BLeft - ANGLE_sweep_val) + LIGHT_left
560 + SONAR_left), ANGLE_mid_BLeft);
561 ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft,
562 ANGLE_min_Shaft, ((ANGLE_mid_BRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right),
563 ANGLE_mid_BRight);
564 }
565 else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev
566 < ANGLE_shaft) {
567 ANGLE_front_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft,
568 ANGLE_mid_FLeft, ((ANGLE_mid_FLeft - ANGLE_sweep_val) + LIGHT_left + SONAR_left));
569
570 ANGLE_front_right = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight,
571 ((ANGLE_mid_FRight - ANGLE_sweep_val) - LIGHT_right - SONAR_right));
572 ANGLE_back_left
573 = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, ((ANGLE_mid_BLeft
574 + ANGLE_sweep_val) - LIGHT_left - SONAR_left));
575 ANGLE_back_right = map(ANGLE_shaft,
576 ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, ((ANGLE_mid_BRight + ANGLE_sweep_val)
577 + LIGHT_right + SONAR_right));
578 }
579
580 SERVO_front_left.writeMicroseconds(ANGLE_front_left);
581
582 SERVO_front_right.writeMicroseconds(ANGLE_front_right);
583 SERVO_back_left.writeMicroseconds(ANGLE_back_left);
584
585 SERVO_back_right.writeMicroseconds(ANGLE_back_right);
586}
587
588
590
591
592
594
595void ROTATE_LEFT() {
596
597 while(flag_transition_rotate == 1) {
598 TRANSITION_GAIT();
599 }
600 flag_transition_rotate
601 = 2;
602
603 while(flag_shaft_reverse == 2) {
604 SHAFT_REVERSE();
605 break;
606
607 }
608 flag_shaft_reverse = 3;
609
610 while(flag_transition_start == 1) {
611
612 SHAFT();
613 LIGHT_left = 0;
614 LIGHT_right = 0;
615 TRANSITION_START();
616
617 ROTATE();
618 }
619
620 SHAFT();
621 LIGHT_left = 0;
622 LIGHT_right =
623 0;
624 ANGLE_sweep_val = ANGLE_sweep;
625 ROTATE();
626}
627
628void ROTATE_RIGHT()
629 {
630 while(flag_transition_rotate == 1) {
631 TRANSITION_GAIT();
632 }
633
634 flag_transition_rotate = 2;
635
636 while(flag_shaft_reverse == 3) {
637 SHAFT_REVERSE();
638
639 break;
640 }
641 flag_shaft_reverse = 2;
642
643 while(flag_transition_start
644 == 1) {
645 SHAFT();
646 LIGHT_left = 0;
647 LIGHT_right = 0;
648 TRANSITION_START();
649
650 ROTATE();
651 }
652
653 SHAFT();
654 LIGHT_left = 0;
655 LIGHT_right =
656 0;
657 ANGLE_sweep_val = (ANGLE_sweep * -1);
658 ROTATE();
659}
660
661void ROTATE()
662 {
663 if(ANGLE_shaft >= ANGLE_mid_Shaft && ANGLE_prev < ANGLE_shaft) {
664 ANGLE_front_left
665 = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_FLeft + ANGLE_sweep_val),
666 ANGLE_mid_FLeft);
667 ANGLE_front_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft,
668 (ANGLE_mid_FRight - ANGLE_sweep_val), ANGLE_mid_FRight);
669 ANGLE_back_left
670 = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft, (ANGLE_mid_BLeft - ANGLE_sweep_val),
671 ANGLE_mid_BLeft);
672 ANGLE_back_right = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_max_Shaft,
673 (ANGLE_mid_BRight + ANGLE_sweep_val), ANGLE_mid_BRight);
674 }
675 else if(ANGLE_shaft
676 >= ANGLE_mid_Shaft && ANGLE_prev > ANGLE_shaft) {
677 ANGLE_front_left = map(ANGLE_shaft,
678 ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FLeft, (ANGLE_mid_FLeft - ANGLE_sweep_val));
679
680 ANGLE_front_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight,
681 (ANGLE_mid_FRight + ANGLE_sweep_val));
682 ANGLE_back_left = map(ANGLE_shaft,
683 ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BLeft, (ANGLE_mid_BLeft + ANGLE_sweep_val));
684
685 ANGLE_back_right = map(ANGLE_shaft, ANGLE_max_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight,
686 (ANGLE_mid_BRight - ANGLE_sweep_val));
687 }
688 else if(ANGLE_shaft < ANGLE_mid_Shaft
689 && ANGLE_prev > ANGLE_shaft) {
690 ANGLE_front_left = map(ANGLE_shaft, ANGLE_mid_Shaft,
691 ANGLE_min_Shaft, (ANGLE_mid_FLeft - ANGLE_sweep_val), ANGLE_mid_FLeft);
692 ANGLE_front_right
693 = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_FRight + ANGLE_sweep_val),
694 ANGLE_mid_FRight);
695 ANGLE_back_left = map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft,
696 (ANGLE_mid_BLeft + ANGLE_sweep_val), ANGLE_mid_BLeft);
697 ANGLE_back_right =
698 map(ANGLE_shaft, ANGLE_mid_Shaft, ANGLE_min_Shaft, (ANGLE_mid_BRight - ANGLE_sweep_val),
699 ANGLE_mid_BRight);
700 }
701 else if(ANGLE_shaft < ANGLE_mid_Shaft && ANGLE_prev
702 < ANGLE_shaft) {
703 ANGLE_front_left = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft,
704 ANGLE_mid_FLeft, (ANGLE_mid_FLeft + ANGLE_sweep_val));
705 ANGLE_front_right
706 = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_FRight, (ANGLE_mid_FRight
707 - ANGLE_sweep_val));
708 ANGLE_back_left = map(ANGLE_shaft, ANGLE_min_Shaft,
709 ANGLE_mid_Shaft, ANGLE_mid_BLeft, (ANGLE_mid_BLeft - ANGLE_sweep_val));
710 ANGLE_back_right
711 = map(ANGLE_shaft, ANGLE_min_Shaft, ANGLE_mid_Shaft, ANGLE_mid_BRight, (ANGLE_mid_BRight
712 + ANGLE_sweep_val));
713 }
714
715 SERVO_front_left.writeMicroseconds(ANGLE_front_left);
716
717 SERVO_front_right.writeMicroseconds(ANGLE_front_right);
718 SERVO_back_left.writeMicroseconds(ANGLE_back_left);
719
720 SERVO_back_right.writeMicroseconds(ANGLE_back_right);
721}
722
723
725
726
729
730
731
733
734void SONAR_READ_ALL()
735 {
736 for(SONAR_id = 0; SONAR_id < SONAR_sum; SONAR_id++) {
737 distance[SONAR_id]
738 = int (SONAR_READ(SONAR_id));
739 }
740}
741
742float SONAR_READ(int index) {
743
744 float SONAR_distance;
745
746 digitalWrite(PIN_trig[index], HIGH);
747 delayMicroseconds(SONAR_TrigSig);
748
749 digitalWrite(PIN_trig[index], LOW);
750
751 float SONAR_EcInterval = float
752 (pulseIn(PIN_ec[index], HIGH, SONAR_MaxEc));
753
754 while(SONAR_EcInterval >
755 0.0) {
756 SONAR_distance = SONAR_EcInterval * (SOUND_speed / 2.0);
757 break;
758
759 }
760 while(SONAR_EcInterval == 0.0) {
761 SONAR_distance = 501.0;
762 break;
763
764 }
765 return SONAR_distance;
766}
767
768
770
771
772
774
775void LIGHT_COMPARE_EXECUTE()
776 {
777
778
779 = analogRead(PIN_PHOTO[FRONT_RIGHT]);
780 PHOTO_Front_Left = analogRead(PIN_PHOTO[FRONT_LEFT]);
781
782 PHOTO_Front_Right = analogRead(PIN_PHOTO[FRONT_RIGHT]);
783 PHOTO_Back_Left =
784 analogRead(PIN_PHOTO[BACK_LEFT]);
785 PHOTO_Back_Right = analogRead(PIN_PHOTO[BACK_RIGHT]);
786
787
788 if((PHOTO_Front_Left + PHOTO_Front_Right) >= (PHOTO_Back_Left + PHOTO_Back_Right))
789 {
790 int LIGHT_Sensitivity = 50;
791 if(LIGHT_COMPARE() > LIGHT_Sensitivity)
792 {
793 LIGHT_left = LIGHT_COMPARE();
794 LIGHT_right = 0;
795 }
796
797 else if(LIGHT_COMPARE() < -LIGHT_Sensitivity) {
798 LIGHT_left = 0;
799
800 LIGHT_right = LIGHT_COMPARE();
801 }
802 else {
803 LIGHT_left
804 = 0;
805 LIGHT_right = 0;
806 }
807 }
808 else {
809 if(PHOTO_Back_Left
810 > PHOTO_Back_Right) {
811 LIGHT_right = 0;
812 LIGHT_left = ANGLE_turnMAX;
813
814 }
815 else if(PHOTO_Back_Left < PHOTO_Back_Right) {
816 LIGHT_right
817 = -ANGLE_turnMAX;
818 LIGHT_left = 0;
819 }
820 else {
821 int
822 RANDOM_back_light = random(0,2);
823
824 if(RANDOM_back_light == 0) {
825
826 LIGHT_right = 0;
827 LIGHT_left = ANGLE_turnMAX;
828 }
829
830 else if(RANDOM_back_light == 1) {
831 LIGHT_right = -ANGLE_turnMAX;
832
833 LIGHT_left = 0;
834 }
835 }
836 }
837}
838
839int LIGHT_COMPARE()
840 {
841 int LIGHT_rate;
842 if(PHOTO_Front_Left > PHOTO_Front_Right) {
843 LIGHT_rate
844 = PHOTO_Front_Left;
845 }
846 else if(PHOTO_Front_Right > PHOTO_Front_Left) {
847
848 LIGHT_rate = PHOTO_Front_Right;
849 }
850 else {
851
852 and comments the other of these variables bellow
853
854
855 LIGHT_rate = PHOTO_Front_Right;
856 }
857
858 int LIGHT_compareRAW = PHOTO_Front_Left
859 - PHOTO_Front_Right;
860 LIGHT_compareRAW = map(LIGHT_compareRAW, -LIGHT_rate,
861 LIGHT_rate, -ANGLE_turnMAX, ANGLE_turnMAX);;
862
863 return LIGHT_compareRAW;
864}
865
866
868
869
870
872
873void RETREAT_AVOID() {
874
875 counter_gait = 0;
876 while(counter_gait <= counter_gait_max) {
877 RETREAT();
878
879 }
880}
881
882void ROTATE_LEFT_AVOID() {
883 counter_gait = 0;
884 rotate_random
885 = 2;
886 while(counter_gait <= counter_gait_max) {
887 ROTATE_LEFT();
888 }
889}
890
891void
892 ROTATE_RIGHT_AVOID() {
893 counter_gait = 0;
894 rotate_random = 2;
895 while(counter_gait
896 <= counter_gait_max) {
897 ROTATE_RIGHT();
898 }
899}
900
901void ROTATE_RANDOM_AVOID()
902 {
903 rotate_random = ROTATE_RANDOM();
904 while(rotate_random == 0) {
905 ROTATE_LEFT_AVOID();
906
907 }
908 while(rotate_random == 1) {
909 ROTATE_RIGHT_AVOID();
910 }
911}
912
913void
914 SIDE_AVOID() {
915 if(distance[LEFT] <= distTurn && distance[RIGHT] > distTurn)
916 {
917 LIGHT_left = 0;
918 LIGHT_right = 0;
919
920 SONAR_left = 0;
921
922 SONAR_right = -(map(distance[LEFT], 0, distTurn, ANGLE_turnMAX, 0));
923 }
924
925 else if(distance[RIGHT] <= distTurn && distance[LEFT] > distTurn) {
926 LIGHT_left
927 = 0;
928 LIGHT_right = 0;
929
930 SONAR_right = 0;
931 SONAR_left =
932 map(distance[RIGHT], 0, distTurn, ANGLE_turnMAX, 0);
933 }
934 else if(distance[LEFT]
935 <= distTurn && distance[RIGHT] <= distTurn) {
936 LIGHT_left = 0;
937 LIGHT_right
938 = 0;
939
940 if(distance[LEFT] < distance[RIGHT]) {
941 SONAR_left =
942 0;
943 SONAR_right = -(map(distance[LEFT], 0, distTurn, ANGLE_turnNARROW, 0));
944
945 }
946 else if(distance[RIGHT] < distance[LEFT]) {
947 SONAR_right =
948 0;
949 SONAR_left = map(distance[RIGHT], 0, distTurn, ANGLE_turnNARROW, 0);
950
951 }
952 }
953 else {
954 SONAR_right = 0;
955 SONAR_left = 0;
956 }
957}
958
959
961
BnTie
2 years ago
How does it know which direction do go to follow the light? cool project, though!!