-
Notifications
You must be signed in to change notification settings - Fork 1
/
main.c
1458 lines (1378 loc) · 45.6 KB
/
main.c
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
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/*
* Title: Line following and collision avoidance robot car II
* Hardware: ATmega32 @ 16 MHz, HC-SR04 Ultrasonic Ranging Modules, SG90 Servo,
* L293D Motor Driver, TCRT5000 Reflective Optical Sensors, ITR9608
* Optical Interrupters, ILI9341 TFT LCD Driver, QMC5883L Magnetic
* Sensor, VL53L0X Time-of-Flight Ranging Sensors
*
* Created: 5-8-2021 14:44:02
* Author : Tim Dorssers
*
* The menu is controlled by the four push buttons. The magnetic sensor needs one
* time calibration, which is done from the 'Calibrate' menu and choosing 'Magneto
* cal'. The calibration data is erased by choosing *Reset bounds*. The TCRT5000
* sensors can be calibrated by choosing 'Start ir cal' and the data is saved by
* choosing 'Stop ir cal'. All data is saved to EEPROM. Configurable settings are
* made from the 'Configuration' menu. The Kp, Ki, Kd and dT parameters used by
* the motor PID control and the front, side and rear collision distances and the
* minimal speed are set from this menu. The 'Test drive' menu is used to show
* the different maneuvers the car can make. The 'Ping' menu option shows the
* sonar and LIDAR data observed by the car. The collision avoidance mode is
* selected by choosing 'Self drive' and the line following mode is selected by
* choosing 'Line Follow'.
*
* PA0/ADC0=TCRT5000#1 PB0/T0=ILI9341 DC PC0/SCL=I2C PD0/RXD=RS232
* PA1/ADC1=TCRT5000#2 PB1/T1=ILI9341 RST PC1/SDA=I2C PD1/TXD=RS232
* PA2/ADC2=TCRT5000#3 PB2/INT2=HC-SR04#3 PC2=L293D 1A PD2/INT0=HC-SR04#1
* PA3/ADC3=TCRT5000#4 PB3/OC0=L293D 1-2EN PC3=L293D 2A PD3/INT1=HC-SR04#2
* PA4/ADC4=TCRT5000#5 PB4/SS=ILI9341 CS PC4=L293D 3A PD4/OC1B=ITR9608#1
* PA5/ADC5=Buttons PB5/MOSI=ILI9341 MOSI PC5=L293D 4A PD5/OC1A=SG90 Servo
* PA6/ADC6=Battery PB6/MISO=ILI9341 MISO PC6=TOF XSHUT PD6/ICP1=ITR9608#2
* PA7/ADC7=NC PB7/SCK=ILI9341 SCK PC7=IR Array PD7/OC2=L293D 3-4EN
*/
#define F_CPU 16000000
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <avr/sfr_defs.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <util/atomic.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include "ili9341.h"
#include "uart.h"
#include "qmc5883l.h"
#include "vl53l0x.h"
#include "i2cmaster.h"
char buffer[26];
// Timer0
#define TIMER0_OVERFLOW_MICROS (64 * 256 / (F_CPU / 1000000L)) // prescaler 64
#define MILLIS_INC (TIMER0_OVERFLOW_MICROS / 1000) // the whole number
#define FRACT_INC ((TIMER0_OVERFLOW_MICROS % 1000) >> 3) // the fractional number
#define FRACT_MAX (1000 >> 3)
volatile uint16_t timer0_overflow_count = 0;
volatile uint16_t timer0_millis = 0;
// ADC
#define ADC_CHANNEL_COUNT 7
#define BUTTON_CHANNEL 5
#define BATTERY_CHANNEL 6
volatile uint8_t adc_values[ADC_CHANNEL_COUNT];
// IR sensor
#define IR_SENSOR_COUNT 5
uint8_t EEMEM nv_adc_min[IR_SENSOR_COUNT] = {45, 45, 45, 45, 45};
uint8_t EEMEM nv_adc_max[IR_SENSOR_COUNT] = {135, 135, 135, 135, 135};
uint8_t adc_min[IR_SENSOR_COUNT], adc_max[IR_SENSOR_COUNT];
typedef enum {CAL_IDLE, CAL_START, CAL_PROGRESS, CAL_STOP, CAL_MAGNETO} cal_state_t;
cal_state_t calibrate_state;
typedef enum {LINE_NONE, LINE_WHITE, LINE_BLACK} line_detect_t;
line_detect_t line_detect;
int8_t linePosition;
uint8_t EEMEM nv_inverse = 0;
bool lineValid, inverse = false;
// Ultrasonic ranging sensor
#define MAX_DISTANCE UINT8_MAX // sets maximum usable sensor measuring distance
#define PING_DELAY (MAX_DISTANCE * 58 / 1000)
#define SONAR_COUNT 3
enum {FRONT_CENTER, SIDE_RIGHT, SIDE_LEFT};
volatile bool ping_done[SONAR_COUNT];
volatile uint16_t ping_values[SONAR_COUNT];
// ToF ranging sensor
VL53L0X_Dev_t tof[2];
enum dev_index {FRONT, REAR};
// Servo PWM, using prescaler of 8
#define TIMER1_TOP ((F_CPU / 50 / 8) - 1) // 50 Hz (20 ms) PWM period
#define SERVO_0DEG ((uint16_t)(F_CPU / (1000 / 0.75) / 8) - 1)
#define SERVO_90DEG ((uint16_t)(F_CPU / (1000 / 1.65) / 8) - 1) // 1.65 ms duty cycle
#define SERVO_180DEG ((uint16_t)(F_CPU / (1000 / 2.55) / 8) - 1)
#define SERVO_INCR15 ((SERVO_180DEG - SERVO_0DEG) / 12)
#define SERVO_30DEG (SERVO_0DEG + (SERVO_INCR15 * 2))
#define SERVO_DELAY 100 // 0.15 Sec/60 Degrees
// Motor PWM
#define PWM_FREQ (F_CPU / (64 * 256UL)) // prescaler 64, fast pwm
#define MAX_RPM (60UL * PWM_FREQ / 20) // Seconds multiplied by PWM frequency divided by slots
int16_t speedMotor1, speedMotor2;
volatile uint8_t rpmMotor1, rpmMotor2;
uint8_t EEMEM nvKp = 150, nvKi = 15, nvKd = 167; // PID control K values multiplied by 100 [Kd = (2 / Ki) / 8]
uint8_t EEMEM nvdT = 20; // PID sample time in milliseconds
uint8_t Kp, Ki, Kd, dT;
// GUI menu
#define MAX_ITEMS 8
#define MENU_ITEM(x, y) (x * MAX_ITEMS + y)
const char PROGMEM str_start[] = "Start ir cal";
const char PROGMEM str_stop[] = "Stop ir cal";
const char PROGMEM str_reset[] = "Reset bounds";
const char PROGMEM str_magneto[] = "Magneto cal";
const char PROGMEM str_left[] = "Go left";
const char PROGMEM str_right[] = "Go right";
const char PROGMEM str_forward[] = "Forward";
const char PROGMEM str_backward[] = "Backward";
const char PROGMEM str_calibrate[] = "Calibrate";
const char PROGMEM str_test_drive[] = "Test drive";
const char PROGMEM str_ping[] = "Ping";
const char PROGMEM str_configure[] = "Configure";
const char PROGMEM str_kp[] = "Set Kp";
const char PROGMEM str_ki[] = "Set Ki";
const char PROGMEM str_kd[] = "Set Kd";
const char PROGMEM str_value[] = "New value: ";
const char PROGMEM str_turn90[] = "Turn 90 cw";
const char PROGMEM str_turn90ccw[] = "Turn 90 ccw";
const char PROGMEM str_line_follow[] = "Line follow";
const char PROGMEM str_self_drive[] = "Self drive";
const char PROGMEM str_dt[] = "Set dT";
const char PROGMEM str_front[] = "Set frontCol";
const char PROGMEM str_side[] = "Set sideColl";
const char PROGMEM str_rear[] = "Set rearColl";
const char PROGMEM str_min_speed[] = "Set minSpeed";
typedef enum {MAIN, SELF_DRIVE, LINE_FOLLOW, CALIBRATE, CONFIGURE, TEST_DRIVE, PING} menu_t;
PGM_P const PROGMEM menu_items[][MAX_ITEMS] = {
{str_self_drive, str_line_follow, str_calibrate, str_configure, str_test_drive, str_ping, NULL, NULL},
{NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL},
{NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL},
{str_start, str_stop, str_magneto, str_reset, NULL, NULL, NULL, NULL},
{str_kp, str_ki, str_kd, str_dt, str_front, str_side, str_rear, str_min_speed},
{str_forward, str_backward, str_left, str_right, str_turn90, str_turn90ccw, NULL, NULL},
{NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL},
};
menu_t menu_index = MAIN;
uint8_t item_index, num_items;
// Visual ping
typedef enum {PING_BEGIN, PING_TURNED, PING_DONE} ping_state_t;
#define SERVO_STEPS 9 // Number of 15 degree steps from 30 to 150 degrees
#define CENTER_START 3 // 75 degrees
#define F_LEFT_START 6 // 120 degrees
uint8_t raw_distance[SERVO_STEPS], min_distance[3], rear_distance;
uint8_t front_tof, s_right_distance, s_left_distance;
enum min_distance_index {F_CENTER, F_RIGHT, F_LEFT};
typedef enum {VP_NONE, VP_COMPLETE, VP_INCOMPLETE} visual_ping_result_t;
// Self drive
typedef enum {SD_STOP, SD_DRIVE, SD_LOOK_AROUND, SD_TURN, SD_REVERSE} self_drive_state_t;
uint8_t turnAngle;
uint16_t currHeading = 0, startHeading = 0;
self_drive_state_t self_drive_state;
uint8_t frontCollDist, sideCollDist, rearCollDist, minSpeed;
uint8_t EEMEM nvFrontCollDist = 40, nvSideCollDist = 40, nvRearCollDist = 20, nvMinSpeed = 40;
// Up icon 17x16
const char up_bits[] PROGMEM = {
0xFF, 0xFF, 0x01, 0x01, 0x00, 0x01, 0x01, 0x01, 0x01, 0x81, 0x03, 0x01,
0x81, 0x03, 0x01, 0xC1, 0x07, 0x01, 0xC1, 0x07, 0x01, 0xE1, 0x0F, 0x01,
0xE1, 0x0F, 0x01, 0xF1, 0x1F, 0x01, 0xF1, 0x1F, 0x01, 0xF9, 0x3F, 0x01,
0xF9, 0x3F, 0x01, 0xFD, 0x7F, 0x01, 0x01, 0x00, 0x01, 0xFF, 0xFF, 0x01,
};
// Down icon 17x16
const char down_bits[] PROGMEM = {
0xFF, 0xFF, 0x01, 0x01, 0x00, 0x01, 0xFD, 0x7F, 0x01, 0xF9, 0x3F, 0x01,
0xF9, 0x3F, 0x01, 0xF1, 0x1F, 0x01, 0xF1, 0x1F, 0x01, 0xE1, 0x0F, 0x01,
0xE1, 0x0F, 0x01, 0xC1, 0x07, 0x01, 0xC1, 0x07, 0x01, 0x81, 0x03, 0x01,
0x81, 0x03, 0x01, 0x01, 0x01, 0x01, 0x01, 0x00, 0x01, 0xFF, 0xFF, 0x01,
};
// Enter icon 35x16
const char enter_bits[] PROGMEM = {
0xFF, 0xFF, 0xFF, 0xFF, 0x07, 0x01, 0x00, 0x00, 0x00, 0x04, 0x01, 0x00,
0x00, 0x00, 0x04, 0x01, 0x0C, 0x00, 0x60, 0x04, 0x01, 0x0F, 0x00, 0x60,
0x04, 0x81, 0x0F, 0x00, 0x60, 0x04, 0xE1, 0x0F, 0x00, 0x60, 0x04, 0xF9,
0xFF, 0xFF, 0x7F, 0x04, 0xF9, 0xFF, 0xFF, 0x7F, 0x04, 0xE1, 0x0F, 0x00,
0x00, 0x04, 0x81, 0x0F, 0x00, 0x00, 0x04, 0x01, 0x0F, 0x00, 0x00, 0x04,
0x01, 0x0C, 0x00, 0x00, 0x04, 0x01, 0x00, 0x00, 0x00, 0x04, 0x01, 0x00,
0x00, 0x00, 0x04, 0xFF, 0xFF, 0xFF, 0xFF, 0x07,
};
// Escape icon 35x16
const char esc_bits[] PROGMEM = {
0xFF, 0xFF, 0xFF, 0xFF, 0x07, 0x01, 0x00, 0x00, 0x00, 0x04, 0xF1, 0x07,
0x00, 0x00, 0x04, 0xF1, 0x07, 0x00, 0x00, 0x04, 0x71, 0x00, 0x00, 0x00,
0x04, 0x71, 0x80, 0x07, 0x3C, 0x04, 0x71, 0xE0, 0x0F, 0x7F, 0x04, 0xF1,
0xE7, 0x88, 0x47, 0x04, 0xF1, 0xE7, 0x83, 0x03, 0x04, 0x71, 0xC0, 0x8F,
0x03, 0x04, 0x71, 0x00, 0x9E, 0x03, 0x04, 0x71, 0x20, 0x9C, 0x47, 0x04,
0xF1, 0xE7, 0x1F, 0x7F, 0x04, 0xF1, 0xC7, 0x07, 0x3E, 0x04, 0x01, 0x00,
0x00, 0x00, 0x04, 0xFF, 0xFF, 0xFF, 0xFF, 0x07,
};
// Car icon 39x61
const char car_bits[] PROGMEM = {
0x00, 0xE0, 0xFF, 0x07, 0x00, 0x00, 0xFE, 0xFF, 0x7F, 0x00, 0x80, 0x1F,
0x00, 0xF0, 0x01, 0xE0, 0x03, 0x00, 0x80, 0x07, 0x70, 0x00, 0x00, 0x00,
0x0E, 0x38, 0x00, 0x00, 0x00, 0x1C, 0x1C, 0x00, 0x00, 0x00, 0x38, 0xFC,
0x03, 0x00, 0xC0, 0x3F, 0xFC, 0x03, 0x00, 0xC0, 0x3F, 0x00, 0x03, 0x00,
0xC0, 0x00, 0x3E, 0x03, 0x00, 0xC0, 0x3C, 0x7F, 0x03, 0x00, 0xC0, 0x7E,
0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03,
0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0,
0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F,
0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00,
0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E,
0x3E, 0x03, 0x00, 0xC0, 0x3C, 0x00, 0x03, 0x00, 0xC0, 0x00, 0xFC, 0x03,
0x00, 0xC0, 0x1F, 0xFC, 0x03, 0x00, 0xC0, 0x1F, 0x0C, 0x00, 0x00, 0x00,
0x18, 0x0C, 0x00, 0x00, 0x00, 0x18, 0x0C, 0x00, 0x00, 0x00, 0x18, 0x0C,
0x00, 0x00, 0x00, 0x18, 0x0C, 0x00, 0x00, 0x00, 0x18, 0xFC, 0x03, 0x00,
0xC0, 0x1F, 0xFC, 0x03, 0x00, 0xC0, 0x1F, 0x00, 0x03, 0x00, 0xC0, 0x00,
0x3E, 0x03, 0x00, 0xC0, 0x3C, 0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03,
0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0,
0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F,
0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00,
0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E,
0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x7F, 0x03, 0x00, 0xC0, 0x7E, 0x3E, 0x03,
0x00, 0xC0, 0x3C, 0x00, 0x03, 0x00, 0xC0, 0x00, 0xFC, 0x03, 0x00, 0xC0,
0x3F, 0xFC, 0x03, 0x00, 0xC0, 0x3F, 0x0C, 0x00, 0x00, 0x00, 0x30, 0x18,
0x00, 0x00, 0x00, 0x38, 0x78, 0x00, 0x00, 0x00, 0x1C, 0xE0, 0x00, 0x00,
0x00, 0x0F, 0xC0, 0x07, 0x00, 0xE0, 0x03, 0x00, 0xFF, 0xC1, 0xFF, 0x00,
0x00, 0xF8, 0xFF, 0x1F, 0x00, };
// Function macros
#define min(a,b) ((a)<(b)?(a):(b))
#define max(a,b) ((a)>(b)?(a):(b))
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#define map(x,in_min,in_max,out_min,out_max) (((x)-(in_min))*((out_max)-(out_min))/((in_max)-(in_min))+(out_min))
#define adc_start() ADCSRA |= _BV(ADSC) // Start conversion
#define ir_stop() PORTC &= ~_BV(PC7) // Turn IR sensor array off
#define ir_start() PORTC |= _BV(PC7) // Turn IR sensor array on
#define lcd_dump_array_P(__s, __p, __n) lcd_dump_array_p(PSTR(__s), __p, __n)
#define lcd_dump_value_P(__s, __v) lcd_dump_value_p(PSTR(__s), __v)
#define uart_dump_array_P(__s, __p, __n) uart_dump_array_p(PSTR(__s), __p, __n)
#define uart_dump_value_P(__s, __v) uart_dump_value_p(PSTR(__s), __v)
#define uart_input_val_P(__s, __v) uart_input_val_p(PSTR(__s), __v)
// Function prototypes
static void lcd_dump_array_p(const char *progmem_s, uint8_t *p, size_t n);
static void lcd_dump_value_p(const char *progmem_s, int16_t val);
static void uart_dump_array_p(const char *progmem_s, uint8_t *p, size_t n);
static void uart_dump_value_p(const char *progmem_s, int16_t val);
static int16_t uart_input_val_p(const char *progmem_s, int16_t val);
// Initialize IR sensor
static void ir_init(void) {
DDRC |= _BV(PC7); // IR sensor array driver as output
eeprom_read_block(&adc_max, &nv_adc_max, sizeof(adc_max));
eeprom_read_block(&adc_min, &nv_adc_min, sizeof(adc_min));
inverse = eeprom_read_byte(&nv_inverse);
}
// Initialize ADC
static void adc_init(void) {
// AVCC with external capacitor at AREF pin and ADC Left Adjust Result
ADMUX = _BV(ADLAR) | _BV(REFS0);
// ADC prescaler of 128, interrupt flag and enable ADC
ADCSRA = _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0) | _BV(ADIE) | _BV(ADEN);
}
// ADC interrupt
ISR(ADC_vect) {
static uint8_t ch = 0;
ir_stop(); // Turn off IR array
adc_values[ch] = ADCH;
// Advance channel, go back to channel 0 at max channel
if (++ch >= ADC_CHANNEL_COUNT)
ch = 0;
ADMUX = (ADMUX & 0xf0) | ch;
//ADMUX = _BV(ADLAR) | ((ch == BATTERY_CHANNEL) ? _BV(REFS1) : 0) | _BV(REFS0) | ch;
}
// Initialize SG-90 servo PWM
static void init_servo(void) {
// Set OC1A as output
DDRD |= _BV(PD5);
// Set Fast PWM mode 14, prescaler of 8, non-inverting mode
TCCR1A |= _BV(WGM11) | _BV(COM1A1);
TCCR1B |= _BV(WGM12) | _BV(WGM13) | _BV(CS11);
// Set PWM period
ICR1 = TIMER1_TOP;
// Set neutral position
OCR1A = SERVO_90DEG;
}
// Sample encoders every millisecond
static void motor_encoder(void) {
static uint8_t count1, count2; // RPM measurement between 12 and 255
static uint8_t prevEnc1, prevEnc2;
count1++;
uint8_t currEnc1 = bit_is_set(PIND, PD4);
// Detect falling edge
if ((currEnc1 ^ prevEnc1) & prevEnc1) {
rpmMotor1 = (uint16_t)MAX_RPM / count1;
count1 = 0;
}
prevEnc1 = currEnc1;
if (count1 == 255) {
rpmMotor1 = 0;
count1 = 0;
}
count2++;
uint8_t currEnc2 = bit_is_set(PIND, PD6);
// Detect falling edge
if ((currEnc2 ^ prevEnc2) & prevEnc2) {
rpmMotor2 = (uint16_t)MAX_RPM / count2;
count2 = 0;
}
prevEnc2 = currEnc2;
if (count2 == 255) {
rpmMotor2 = 0;
count2 = 0;
}
}
// Motor PID control (every dT milliseconds)
static void motor_pid(void) {
static int32_t intErr1 = 0, intErr2 = 0;
static int16_t prevErr1 = 0, prevErr2 = 0;
if (speedMotor1) {
int16_t proErr = abs(speedMotor1) - rpmMotor1; // proportional term
int16_t derErr = proErr - prevErr1; // derivative term
prevErr1 = proErr; // save error for next pass
intErr1 += proErr; // integral term
int16_t pid = (proErr * Kp + intErr1 * Ki + derErr * Kd) / 100;
OCR0 = constrain(pid, 0, 255);
if (speedMotor1 < 0) { // Backward
PORTC &= ~(1 << PC2);
PORTC |= (1 << PC3);
} else { // Forward
PORTC &= ~(1 << PC3);
PORTC |= (1 << PC2);
}
} else {
// Brake
intErr1 = prevErr1 = 0;
PORTC &= ~(1 << PC2);
PORTC &= ~(1 << PC3);
OCR0 = 0;
}
if (speedMotor2) {
int16_t proErr = abs(speedMotor2) - rpmMotor2; // proportional term
int16_t derErr = proErr - prevErr2; // derivative term
prevErr2 = proErr; // save error for next pass
intErr2 += proErr; // integral term
int16_t pid = (proErr * Kp + intErr2 * Ki + derErr * Kd) / 100;
OCR2 = constrain(pid, 0, 255);
if (speedMotor2 < 0) { // Backward
PORTC &= ~(1 << PC5);
PORTC |= (1 << PC4);
} else { // Forward
PORTC &= ~(1 << PC4);
PORTC |= (1 << PC5);
}
} else {
// Brake
intErr2 = prevErr2 = 0;
PORTC &= ~(1 << PC4);
PORTC &= ~(1 << PC5);
OCR2 = 0;
}
}
// Timer0 interrupt
ISR(TIMER0_OVF_vect) {
static uint8_t timer0_fract = 0;
static uint16_t sample_start = 0;
timer0_millis += MILLIS_INC;
timer0_fract += FRACT_INC;
if (timer0_fract >= FRACT_MAX) {
timer0_fract -= FRACT_MAX;
timer0_millis++;
}
timer0_overflow_count++;
// Motor control
motor_encoder();
if (timer0_millis - sample_start > dT) {
sample_start = timer0_millis;
motor_pid();
}
// Turn on IR array and start ADC conversion
ir_start();
adc_start();
}
static uint16_t millis() {
uint16_t m;
ATOMIC_BLOCK(ATOMIC_FORCEON) {
m = timer0_millis;
}
return m;
}
static uint32_t micros() {
uint16_t m;
uint8_t t;
ATOMIC_BLOCK(ATOMIC_FORCEON) {
m = timer0_overflow_count;
t = TCNT0;
if ((TIFR & _BV(TOV0)) && (t < 255))
m++;
}
return (((uint32_t)m << 8) + t) * (64 / (F_CPU / 1000000L));
}
// Initialize timer0 and motor PWM channels
static void init_timer0(void) {
// Timer0 Clock Select Prescaler 64, Mode 3 Fast PWM, non-inverting
TCCR0 |= _BV(CS00) | _BV(CS01) | _BV(WGM00) | _BV(WGM01) | _BV(COM01);
// Timer0 Overflow Interrupt Enable
TIMSK |= _BV(TOIE0);
// Timer2 Clock Select Prescaler 64, Mode 3 Fast PWM, non-inverting
TCCR2 |= _BV(CS22) | _BV(WGM20) |_BV(WGM21) | _BV(COM21);
// Set OC0 and OC2 as output
DDRB |= _BV(PB3);
DDRD |= _BV(PD7);
}
// Initialize HC-SR04
static void init_sonar(void) {
// Any logical change on INT0 and INT1 generates an interrupt request
MCUCR |= _BV(ISC00) | _BV(ISC10);
}
// Trigger HC-SR04
static void trigger_sonar0(void) {
DDRD |= _BV(PD2); // Configure pin as output
// send 10us trigger pulse
PORTD &= ~_BV(PD2);
_delay_us(4);
PORTD |= _BV(PD2);
_delay_us(10);
PORTD &= ~_BV(PD2);
DDRD &= ~_BV(PD2); // Configure pin as input
GICR |= _BV(INT0); // External Interrupt Request 0 Enable
}
static void trigger_sonar1(void) {
DDRD |= _BV(PD3); // Configure pin as output
// send 10us trigger pulse
PORTD &= ~_BV(PD3);
_delay_us(4);
PORTD |= _BV(PD3);
_delay_us(10);
PORTD &= ~_BV(PD3);
DDRD &= ~_BV(PD3); // Configure pin as input
GICR |= _BV(INT1); // External Interrupt Request 1 Enable
}
static void trigger_sonar2(void) {
DDRB |= _BV(PB2); // Configure pin as output
// send 10us trigger pulse
PORTB &= ~_BV(PB2);
_delay_us(4);
PORTB |= _BV(PB2);
_delay_us(10);
PORTB &= ~_BV(PB2);
DDRB &= ~_BV(PB2); // Configure pin as input
MCUCSR |= _BV(ISC2); // Generate interrupt on rising edge
GICR |= _BV(INT2); // External Interrupt Request 2 Enable
}
// HC-SR04 echo pin interrupt
ISR(INT0_vect) {
static uint32_t ping_start;
if (bit_is_set(PIND, PD2)) {
// rising edge
ping_start = micros(); // start counting
ping_done[FRONT_CENTER] = false; // clear flag
} else if (!ping_done[FRONT_CENTER]) {
// falling edge
ping_values[FRONT_CENTER] = (micros() - ping_start) / 58;
ping_done[FRONT_CENTER] = true; // set flag
GICR &= ~_BV(INT0);
}
}
ISR(INT1_vect) {
static uint32_t ping_start;
if (bit_is_set(PIND, PD3)) {
// rising edge
ping_start = micros(); // start counting
ping_done[SIDE_RIGHT] = false; // clear flag
} else if (!ping_done[SIDE_RIGHT]) {
// falling edge
ping_values[SIDE_RIGHT] = (micros() - ping_start) / 58;
ping_done[SIDE_RIGHT] = true; // set flag
GICR &= ~_BV(INT1);
}
}
ISR(INT2_vect) {
static uint32_t ping_start;
if (bit_is_set(PINB, PB2)) {
// rising edge
ping_start = micros(); // start counting
ping_done[SIDE_LEFT] = false; // clear flag
MCUCSR &= ~_BV(ISC2); // Generate interrupt on falling edge
} else if (!ping_done[SIDE_LEFT]) {
// falling edge
ping_values[SIDE_LEFT] = (micros() - ping_start) / 58;
ping_done[SIDE_LEFT] = true; // set flag
GICR &= ~_BV(INT2);
}
}
// Initialize L293D and ITR9608 pins
static void init_motor(void) {
// Motor activation pins as output
DDRC |= _BV(PC2) | _BV(PC3) | _BV(PC4) | _BV(PC5);
// Enable pull up on encoder pins
PORTB |= _BV(PD4) | _BV(PD6);
// Read PID values from EEPROM
Kp = eeprom_read_byte(&nvKp);
Ki = eeprom_read_byte(&nvKi);
Kd = eeprom_read_byte(&nvKd);
dT = eeprom_read_byte(&nvdT);
}
// Init ToF sensors
static void init_tof(void) {
memset(&tof, 0, sizeof(tof));
DDRC |= _BV(PC6); // XSHUT pin as output
PORTC &= ~_BV(PC6); // set XSHUT low to reset
setTimeout(&tof[FRONT], 100);
uart_puts_P("VL53L0X #1 ");
bool success = initVL53L0X(&tof[FRONT]);
setAddress(&tof[FRONT], 0x30);
if (!success)
if (!initVL53L0X(&tof[FRONT]))
uart_puts_P("fail ");
//setLongRangeMode(&tof[FRONT]);
startContinuous(&tof[FRONT]);
DDRC &= ~_BV(PC6); // XSHUT pin as input
setTimeout(&tof[REAR], 100);
uart_puts_P("VL53L0X #2 ");
if (!initVL53L0X(&tof[REAR]))
uart_puts_P("fail ");
setAddress(&tof[REAR], 0x31);
//setLongRangeMode(&tof[REAR]);
startContinuous(&tof[REAR]);
}
// Dump array of uint8_t to LCD with label string stored in progmem
static void lcd_dump_array_p(const char *progmem_s, uint8_t *p, size_t n) {
ili9341_puts_p(progmem_s);
while (n--) {
utoa(*p++, buffer, 10);
ili9341_setTextColor(ILI9341_YELLOW, ILI9341_DARKBLUE);
ili9341_puts(buffer);
ili9341_setTextColor(ILI9341_WHITE, ILI9341_DARKBLUE);
if (n)
ili9341_puts_p(PSTR(", "));
}
}
// Dump int16_t to LCD with label string stored in progmem
static void lcd_dump_value_p(const char *progmem_s, int16_t val) {
ili9341_puts_p(progmem_s);
itoa(val, buffer, 10);
ili9341_setTextColor(ILI9341_YELLOW, ILI9341_DARKBLUE);
ili9341_puts(buffer);
ili9341_setTextColor(ILI9341_WHITE, ILI9341_DARKBLUE);
}
// Dump array of uint8_t to UART with label string stored in progmem
static void uart_dump_array_p(const char *progmem_s, uint8_t *p, size_t n) {
uart_puts_p(progmem_s);
while (n--) {
utoa(*p++, buffer, 10);
uart_puts(buffer);
if (n)
uart_puts_P(", ");
}
}
// Dump int16_t to UART with label string stored in progmem
static void uart_dump_value_p(const char *progmem_s, int16_t val) {
uart_puts_p(progmem_s);
itoa(val, buffer, 10);
uart_puts(buffer);
}
// Get a new value for an int16_t from UART with label string stored in progmem
static int16_t uart_input_val_p(const char *progmem_s, int16_t val) {
uint8_t i = 0, c;
uart_dump_value_p(progmem_s, val);
uart_puts_P("\r\nNew value: ");
do {
while (!uart_available()); // Wait for character
c = uart_getc();
if (c == '-' || (c >= '0' && c <= '9')) { // Numeric character
uart_putc(c);
buffer[i++] = c;
}
if ((c == 8 || c == 127) && i > 0) { // Backspace
uart_putc(c);
i--;
}
} while (c != 13 && i <= sizeof(buffer) - 1); // Enter
buffer[i] = 0;
uart_puts_P("\r\n");
return (int16_t)atoi(buffer);
}
// Dump motor status to UART
void dump_status(void) {
if (speedMotor1 < speedMotor2)
uart_puts_P("right, ");
else if (speedMotor1 > speedMotor2)
uart_puts_P("left, ");
else if (speedMotor1 < 0 && speedMotor2 < 0)
uart_puts_P("backward, ");
else if (speedMotor1 == 0 && speedMotor2 == 0)
uart_puts_P("stop, ");
else
uart_puts_P("forward, ");
uart_dump_value_P("motor=", speedMotor1);
uart_dump_value_P(", ", speedMotor2);
uart_puts_P("\r\n");
}
// Draw a two column menu and invert colors of selected item
static void drawMenu(void) {
PGM_P str;
uint8_t i, x = 0, y = 32;
for (i = 0; i < MAX_ITEMS; i++) {
memcpy_P(&str, &menu_items[menu_index][i], sizeof(PGM_P));
if (str == NULL)
break;
if (item_index == i)
ili9341_setTextColor(ILI9341_DARKBLUE, ILI9341_WHITE);
else
ili9341_setTextColor(ILI9341_WHITE, ILI9341_DARKBLUE);
ili9341_setCursor(x, y);
ili9341_puts_p(str);
y += 16;
if (y > 80) {
y = 32;
x += 143;
}
}
num_items = i; // Number of items in current menu
ili9341_setTextColor(ILI9341_WHITE, ILI9341_DARKBLUE);
}
// Draw initial screen
static void drawScreen(void) {
ili9341_fillScreen(ILI9341_DARKBLUE);
drawMenu();
if (num_items) {
ili9341_drawhline(0, 24, 275, ILI9341_WHITE);
ili9341_setTextColor(ILI9341_YELLOW, ILI9341_DARKBLUE);
ili9341_setCursor(106, 16);
ili9341_puts_p(PSTR("Menu"));
ili9341_drawhline(0, 104, 275, ILI9341_WHITE);
ili9341_drawXBitmapTrans(303, 16, up_bits, 17, 16, ILI9341_WHITE);
ili9341_drawXBitmapTrans(303, 80, down_bits, 17, 16, ILI9341_WHITE);
ili9341_drawXBitmapTrans(285, 160, enter_bits, 35, 16, ILI9341_WHITE);
} else
ili9341_drawXBitmapTrans(172, 136, car_bits, 39, 61, ILI9341_WHITE);
ili9341_drawXBitmapTrans(285, 224, esc_bits, 35, 16, ILI9341_WHITE);
ili9341_setTextColor(ILI9341_WHITE, ILI9341_DARKBLUE);
}
// Calculate line position and visualize IR channels
static void visual_ir(void) {
static uint8_t previous[IR_SENSOR_COUNT];
uint32_t numeratorSum = 0;
uint16_t denominatorSum = 0;
uint8_t cnt = 0;
bool draw = false;
// Count sensors detecting line
for (uint8_t i = 0; i < IR_SENSOR_COUNT; i++) {
uint8_t threshold = (adc_min[i] + adc_max[i]) / 2;
uint8_t value = adc_values[i];
if (value > threshold)
cnt++;
if (previous[i] != value)
draw = true;
previous[i] = value;
}
if (cnt == 0 || cnt == IR_SENSOR_COUNT) {
line_detect = LINE_NONE;
} else if (cnt > 0 && cnt <= IR_SENSOR_COUNT / 2) {
line_detect = LINE_WHITE;
} else {
line_detect = LINE_BLACK;
}
for (uint8_t i = 0; i < IR_SENSOR_COUNT; i++) {
// Constrain to min and max values
uint8_t normalized = previous[i];
normalized = min(normalized, adc_max[i]);
normalized = max(normalized, adc_min[i]);
// Map to 0 to 255 range
normalized = map((uint16_t)normalized, adc_min[i], adc_max[i], 0, 255);
// Visualize
if (draw) {
uint16_t color = color565(normalized, normalized, normalized);
ili9341_fillrect((IR_SENSOR_COUNT - i - 1) * (275 / IR_SENSOR_COUNT), 0, 275 / IR_SENSOR_COUNT, 16, color);
}
// White line on dark surface
if (inverse)
normalized = 255 - normalized;
// Calculate weighted average of each sensor channel
numeratorSum += (uint32_t)normalized * 64 * i;
denominatorSum += normalized;
}
if (denominatorSum == 0 || denominatorSum == IR_SENSOR_COUNT * 255) {
lineValid = false;
} else {
// 5 channels is between -128 and 127
linePosition = 64 * (IR_SENSOR_COUNT - 1) / 2 - (numeratorSum / denominatorSum);
lineValid = true;
}
// Visualize
if (linePosition > -69)
ili9341_fillrect(137 - linePosition * 2, 0, 2, 16, ILI9341_RED);
}
// Ping surroundings, calculate distances and visualize on screen
static visual_ping_result_t visual_ping(uint8_t range) {
static uint8_t index = CENTER_START;
static bool reverse = false;
static ping_state_t ping_state = PING_BEGIN;
static uint16_t startTime;
uint8_t minStep, maxStep;
visual_ping_result_t result = VP_NONE;
minStep = (SERVO_STEPS - 1) / 2 - range;
maxStep = (SERVO_STEPS - 1) / 2 + range;
index = constrain(index, minStep, maxStep);
switch (ping_state) {
case PING_BEGIN:
// Set next servo angle
index += (reverse) ? -1 : 1;
OCR1A = SERVO_30DEG + (SERVO_INCR15 * index);
startTime = millis();
trigger_sonar1(); // Right sonar
// Drawing three lines takes about 50 ms
ili9341_setCursor(0, 16);
lcd_dump_value_P("fl=", min_distance[F_LEFT]);
lcd_dump_value_P(" fc=", min_distance[F_CENTER]);
lcd_dump_value_P(" fr=", min_distance[F_RIGHT]);
ili9341_clearTextArea(275);
ili9341_setCursor(0, 32);
lcd_dump_value_P("sr=", s_right_distance);
lcd_dump_value_P(" sl=", s_left_distance);
lcd_dump_value_P(" rc=", rear_distance);
ili9341_clearTextArea(319);
ili9341_setCursor(0, 48);
lcd_dump_value_P("speed1=", speedMotor1);
lcd_dump_value_P(" speed2=", speedMotor2);
ili9341_clearTextArea(319);
trigger_sonar2(); // Left sonar
ping_state = PING_TURNED;
break;
case PING_TURNED:
// Has the servo been given enough time to run?
if (millis() - startTime < SERVO_DELAY)
break;
// Reverse direction
if (index == maxStep)
reverse = true;
if (index == minStep)
reverse = false;
trigger_sonar0(); // Center sonar
startTime = millis();
// LIDAR
uint16_t raw_range = readRangeContinuousMillimeters(&tof[FRONT]) / 10;
front_tof = (raw_range > MAX_DISTANCE) ? MAX_DISTANCE : raw_range;
raw_range = readRangeContinuousMillimeters(&tof[REAR]) / 10;
rear_distance = (raw_range > MAX_DISTANCE) ? MAX_DISTANCE : raw_range;
if (timeoutOccurred(&tof[FRONT]) || timeoutOccurred(&tof[REAR]))
init_tof();
ping_state = PING_DONE;
break;
case PING_DONE:
// Has the ultrasound been given enough time to reach us?
if (millis() - startTime < PING_DELAY)
break;
// Check sonar readings
for (uint8_t i = 0; i < 3; i++) {
if (!ping_done[i] || ping_values[i] > MAX_DISTANCE) {
ping_done[i] = true;
ping_values[i] = MAX_DISTANCE;
}
}
// Visualizing IR sensors takes about 10 ms
visual_ir();
// Erase previous front visualization
int8_t angle = 60 - index * 15;
ili9341_drawLineByAngle(191, 136, angle, raw_distance[index] / 4, ILI9341_DARKBLUE);
// Visualize front (sonar in Grey and LIDAR in red)
raw_distance[index] = ping_values[FRONT_CENTER];
uint16_t color = ILI9341_GRAY;
if (front_tof < raw_distance[index]) {
raw_distance[index] = front_tof;
color = ILI9341_RED;
}
if (raw_distance[index] < MAX_DISTANCE)
ili9341_drawLineByAngle(191, 136, angle, raw_distance[index] / 4, color);
// Visualize side right
s_right_distance = ping_values[SIDE_RIGHT];
uint8_t temp = (s_right_distance == MAX_DISTANCE) ? 0 : s_right_distance;
ili9341_drawhline(211, 166, temp / 4, ILI9341_GRAY);
ili9341_drawhline(211 + temp / 4, 166, 64 - temp / 4, ILI9341_DARKBLUE);
// Visualize side left
s_left_distance = ping_values[SIDE_LEFT];
temp = (s_left_distance == MAX_DISTANCE) ? 0 : s_left_distance;
ili9341_drawhline(172 - temp / 4, 166, temp / 4, ILI9341_GRAY);
ili9341_drawhline(172 - 64 - temp / 4, 166, 64 - temp / 4, ILI9341_DARKBLUE);
// Visualize rear
temp = (rear_distance == MAX_DISTANCE) ? 0 : rear_distance;
ili9341_drawvline(191, 197, temp / 4, ILI9341_RED);
ili9341_drawvline(191, 197 + temp / 4, 64 - temp / 4, ILI9341_DARKBLUE);
// Compute minimal distances for 30-60, 75-105 and 120-150 degrees
memset(min_distance, MAX_DISTANCE, sizeof(min_distance));
result = VP_COMPLETE;
for (uint8_t i = 0; i < SERVO_STEPS - 1; i++) {
if (i < minStep || i > maxStep) {
ili9341_drawLineByAngle(191, 136, 60 - i * 15, raw_distance[i] / 4, ILI9341_DARKBLUE);
raw_distance[i] = 0;
continue;
} else if (raw_distance[i] == 0)
result = VP_INCOMPLETE;
if (i < CENTER_START && raw_distance[i] < min_distance[F_RIGHT])
min_distance[F_RIGHT] = raw_distance[i];
if (i >= CENTER_START && i < F_LEFT_START && raw_distance[i] < min_distance[F_CENTER])
min_distance[F_CENTER] = raw_distance[i];
if (i >= F_LEFT_START && raw_distance[i] < min_distance[F_LEFT])
min_distance[F_LEFT] = raw_distance[i];
}
uart_dump_value_P("tof=", front_tof);
uart_dump_array_P(" distance=", raw_distance, SERVO_STEPS);
uart_puts_P("\r\n");
ping_state = PING_BEGIN;
break;
}
return result;
}
// Shortest distance between two angles
static uint16_t angular_distance(uint16_t a, uint16_t b) {
uint16_t d = abs(a - b);
return d > 180 ? 360 - d : d;
}
// Set desired turn angle
static void set_angle(uint8_t newAngle) {
turnAngle = newAngle;
startHeading = qmc5883l_get_heading();
}
// Check if maneuver is done
static bool maneuver(void) {
currHeading = qmc5883l_get_heading();
if (turnAngle && (rpmMotor1 || rpmMotor2)) {
if (angular_distance(startHeading, currHeading) >= turnAngle) {
turnAngle = 0;
currHeading = 0;
return true;
}
}
return false;
}
// Collision avoidance algorithm
static void self_drive(void) {
switch (self_drive_state) {
case SD_STOP:
speedMotor1 = speedMotor2 = 0;
break;
case SD_DRIVE:
if (visual_ping(1) != VP_COMPLETE)
break;
speedMotor1 = speedMotor2 = constrain(min_distance[F_CENTER], minSpeed, 128);
if (min_distance[F_CENTER] < frontCollDist)
self_drive_state = SD_LOOK_AROUND;
else {
// Veer away from side objects
uint8_t min_side = min(sideCollDist, (s_right_distance + s_left_distance) / 2);
if (s_right_distance < min_side) {
uint8_t temp = min_side - s_right_distance;
speedMotor1 += temp;
speedMotor2 -= temp;
}
if (s_left_distance < min_side) {
uint8_t temp = min_side - s_left_distance;
speedMotor1 -= temp;
speedMotor2 += temp;
}
}
break;
case SD_LOOK_AROUND:
// Stand still if too close to object
if (min_distance[F_CENTER] < frontCollDist / 2 ||
min_distance[F_RIGHT] < frontCollDist / 2 || min_distance[F_LEFT] < frontCollDist / 2)
speedMotor1 = speedMotor2 = 0;
// Wait for full sweep to finish
if (visual_ping(4) != VP_COMPLETE)
break;
// Front object blocking all directions?
if (min_distance[F_RIGHT] < frontCollDist && min_distance[F_LEFT] < frontCollDist) {
// Object on both sides?
if (s_right_distance < sideCollDist && s_left_distance < sideCollDist) {
// Rear object?
self_drive_state = (rear_distance < rearCollDist) ? SD_STOP : SD_REVERSE;
} else {
if (s_right_distance < s_left_distance) {
// turn left
speedMotor1 = minSpeed;
speedMotor2 = -minSpeed;
} else {
// turn right
speedMotor1 = -minSpeed;
speedMotor2 = minSpeed;
}
set_angle(90);
self_drive_state = SD_TURN;
}
} else {
if (min_distance[F_RIGHT] < min_distance[F_LEFT]) {
// turn left
speedMotor1 = minSpeed;
speedMotor2 = (s_left_distance < sideCollDist) ? 0 : -minSpeed;
} else {
// turn right
speedMotor1 = (s_right_distance < sideCollDist) ? 0 : -minSpeed;
speedMotor2 = minSpeed;
}
set_angle(22);
self_drive_state = SD_TURN;
}
break;
case SD_TURN:
visual_ping(0);
ili9341_setCursor(0, 64);
if (maneuver()) {
self_drive_state = SD_DRIVE;
} else {
lcd_dump_value_P("heading=", currHeading);
lcd_dump_value_P(" turn=", turnAngle);
}
ili9341_clearTextArea(319);
break;
case SD_REVERSE:
visual_ping(0);
speedMotor1 = speedMotor2 = -constrain(rear_distance, minSpeed, 128);
// One side cleared?
if (s_right_distance > sideCollDist || s_left_distance > sideCollDist) {
if (s_right_distance > s_left_distance) {
// turn left
speedMotor1 = -minSpeed;
speedMotor2 = minSpeed;
} else {
// turn right
speedMotor1 = minSpeed;
speedMotor2 = -minSpeed;
}
set_angle(90);
self_drive_state = SD_TURN;
}
if (rear_distance < rearCollDist)
self_drive_state = SD_DRIVE;
break;
}
ili9341_setCursor(0, 80);
lcd_dump_value_P("state=", self_drive_state);
//ili9341_clearTextArea(275);
}
// Perform auto calibration
static void auto_calibrate(void) {
switch (calibrate_state) {
case CAL_IDLE:
break;
case CAL_START: