-
Notifications
You must be signed in to change notification settings - Fork 0
/
SilverRover.ino
2156 lines (2005 loc) · 65.7 KB
/
SilverRover.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
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
#include <Wire.h>
#include "SerialUART.h"
#include <HCSR04.h>
#include <EEPROM.h>
#include <Servo.h>
#include <i2cdetect.h>
#include <Adafruit_PWMServoDriver.h>
#include "I2CScanner.h"
#include "Adafruit_VL53L0X.h"
I2CScanner scanner;
Servo radar_servo;
#define SERVO_FREQ 60 // Analog servos run at ~50 Hz updates
#define STERZO_SINISTRA_MAX 220
#define STERZO_DRITTO 330//328
#define STERZO_DESTRA_MAX 440
#define DRITTO STERZO_DRITTO
#define CHIUSO 0
#define APERTO 1
#define META 2
#define ATTIVATO 1
#define DISATTIVATO 0
#define reset_button 22
#define LOX1_ADDRESS 0x30
#define LOX2_ADDRESS 0x31
#define LOX3_ADDRESS 0x32
#define LOX4_ADDRESS 0x33
#define LOX5_ADDRESS 0x34
#define LOX6_ADDRESS 0x35
//#define LOX4_ADDRESS 0x33
int sensor1,sensor2, sensor3, sensor4, sensor5, sensor6;
#define SHT_LOX1 11
#define SHT_LOX2 12
#define SHT_LOX3 13
#define SHT_LOX4 10
#define SHT_LOX5 9
#define SHT_LOX6 8
//#define SHT_LOX4
Adafruit_VL53L0X lox1 = Adafruit_VL53L0X();
Adafruit_VL53L0X lox2 = Adafruit_VL53L0X();
Adafruit_VL53L0X lox3 = Adafruit_VL53L0X();
Adafruit_VL53L0X lox4 = Adafruit_VL53L0X();
Adafruit_VL53L0X lox5 = Adafruit_VL53L0X();
Adafruit_VL53L0X lox6 = Adafruit_VL53L0X();
VL53L0X_RangingMeasurementData_t measure1;
VL53L0X_RangingMeasurementData_t measure2;
VL53L0X_RangingMeasurementData_t measure3;
VL53L0X_RangingMeasurementData_t measure4;
VL53L0X_RangingMeasurementData_t measure5;
VL53L0X_RangingMeasurementData_t measure6;
//----------------------------------------------------------------------------------------------------------------------
//#include "FaBoPWM_PCA9685.h"
//#include <HCSR04.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();//----------------------------------------------------------------------------------------------------
//HCSR04 hc(5,new int[4]{3,2,4,6},4);
//FaBoPWM faboPWM;
const uint16_t STOP = 1500;
const uint16_t FORWARD = 2000;
const uint16_t BACK = 1000;
const uint16_t run_delay = 2100; //2100 percorre circa 50cm
uint8_t POSIZIONE_PANNELLO_DX = CHIUSO;
uint8_t POSIZIONE_PANNELLO_SX = CHIUSO;
uint8_t peso_destra = 0;
uint8_t peso_sinistra = 0;
uint8_t stato_precedente = CHIUSO;
const uint8_t panel_speed = 5;
//uint16_t radar_scan[13] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
//uint16_t radar_scan_motor[48] = {110, 120, 130, 140, 150, 160, 170, 180 , 190, 200 , 210, 220, 230, 240, 250, 260, 270, 280, 290, 300, 310, 320, 330, 340, 350, 360, 370, 380, 390, 400, 410, 420, 430, 440, 450, 460, 470, 480, 490, 500, 510, 520, 530, 540, 550, 560, 570, 580};
uint16_t radar_scan_180[48];
uint8_t webcam_sterzo = ATTIVATO;
uint8_t radar_sterzo = DISATTIVATO;
uint8_t pca_off = ATTIVATO;
uint16_t posizione_sterzo = STERZO_DRITTO;
uint16_t webcam_sopra_sotto = 250;
uint16_t webcam_sinistra_destra = 268;
uint16_t webcam_sopra_sotto_effettuato = DISATTIVATO;
uint8_t risparmio_energetico = DISATTIVATO;
uint8_t sterzo_automatico = DISATTIVATO;
uint16_t radar_scan_posizion = DRITTO;
uint8_t radar_direction = 0;
uint8_t avviso_ant = 0;
uint8_t avviso_dx_ant_45 = 0;
uint8_t avviso_sx_ant_45 = 0;
uint8_t avviso_dx_center = 0;
uint8_t avviso_sx_center = 0;
uint8_t motor_speed_id = 10;
uint16_t radar_delay = 100;
//int array_valore_sterzo[10] = {55,
uint16_t precisione_sterzo = 7;
uint16_t valore_sterzata = 11;//precisione_sterzo;
int8_t webcam_X = 0;
int8_t webcam_Y = 0;
uint8_t ruote_sterzanti_post = ATTIVATO;
uint8_t ruote_sterzanti_ant = ATTIVATO;
uint16_t radar_position = 360;
uint16_t radar_dx_ant_position = 110;
uint16_t radar_sx_ant_position = 580;
uint16_t panel_position_dx = 0;
uint16_t panel_position_sx = 0;
uint8_t panels_security_distance = 0;
uint8_t security_distance = 250;
int8_t avanzamento_c = DISATTIVATO;
int8_t anticollisione = ATTIVATO;
int radar_dx_ant = 0;
int radar_dx_ant_aux = 0;
int radar_dx_center = 0;
int radar_dx_ant_45 = 0;
int radar_dx_ant_45_aux = 0;
int radar_sx_ant = 0;
int radar_sx_ant_aux = 0;
int radar_sx_center = 0;
int radar_sx_ant_45 = 0;
int radar_sx_ant_45_aux = 0;
int radar_ant = 0;
int radar_ant_aux = 0;
uint16_t radar_motor_speed = 400;
int16_t motor_speed = 0;
int16_t ant_sx_speed = 0;
int16_t ant_dx_speed = 0;
int16_t post_dx_speed = 0;
int16_t post_sx_speed = 0;
int16_t trimmer = 0;
int16_t limite_sinistro = STERZO_SINISTRA_MAX + trimmer;
int16_t limite_destro = STERZO_DESTRA_MAX + trimmer;
int16_t centro = STERZO_DRITTO + trimmer;
int radar_post = 0;
uint16_t task = radar_motor_speed;
//VoltMeter==============================
int analogInput_1 = analogRead(27);
int analogInput_2 = analogRead(26);
float Vout_1 = 0.00;
float Vin_1 = 0.00;
float Vout_2 = 0.00;
float Vin_2 = 0.00;
float input_voltage_1 = 0.0;
float input_voltage_2 = 0.0;
float temp_1 = 0.0;
float temp_2 = 0.0;
float R1_1 = 30000.00; // resistance of R1 (30K)
float R2_1 = 7500.00; // resistance of R2 (7.5K)
float R1_2 = 30000.00; // resistance of R1 (30K)
float R2_2 = 7500.00; // resistance of R2 (7.5K)
int val_1 = 0;
int val_2 = 0;
uint32_t tempo_funzionamento = millis();
uint32_t inizio_conteggio_distanza = 0;
uint32_t fine_conteggio_distanza = 0;
float distanza_percorsa;
int post_sx_speed_2 = 0;
bool costeggia_muro_enabled = false;
const int chipSelect = 10;
uint8_t diagnostic = DISATTIVATO;
void setID() {
///Robojax.com code see video https://youtu.be/0glBk917HPg
// all reset
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
digitalWrite(SHT_LOX3, LOW);
digitalWrite(SHT_LOX4, LOW);
digitalWrite(SHT_LOX5, LOW);
digitalWrite(SHT_LOX6, LOW);
delay(10);
// all unreset
digitalWrite(SHT_LOX1, HIGH);
digitalWrite(SHT_LOX2, HIGH);
digitalWrite(SHT_LOX3, HIGH);
digitalWrite(SHT_LOX4, HIGH);
digitalWrite(SHT_LOX5, HIGH);
digitalWrite(SHT_LOX6, HIGH);
delay(10);
// activating LOX1 and reseting other sensors
digitalWrite(SHT_LOX1, HIGH);
digitalWrite(SHT_LOX2, LOW);
digitalWrite(SHT_LOX3, LOW);
digitalWrite(SHT_LOX4, LOW);
digitalWrite(SHT_LOX5, LOW);
digitalWrite(SHT_LOX6, LOW);
// initing LOX1
if(!lox1.begin(LOX1_ADDRESS)) {
Serial.println(F("Failed to boot 1 VL53L0X"));
//while(1);
}
delay(10);
// activating LOX2
digitalWrite(SHT_LOX2, HIGH);
delay(10);
//initing LOX2
if(!lox2.begin(LOX2_ADDRESS)) {
Serial.println(F("Failed to boot 2 VL53L0X"));
//while(1);
}
// activating LOX3
digitalWrite(SHT_LOX3, HIGH);
delay(10);
//initing LOX3
if(!lox3.begin(LOX3_ADDRESS)) {
Serial.println(F("Failed to boot 3 VL53L0X"));
//while(1);
}
//activating LOX4
digitalWrite(SHT_LOX4, HIGH);
delay(10);
//initing LOX4
if(!lox4.begin(LOX4_ADDRESS)) {
Serial.println(F("Failed to boot 4 VL53L0X"));
//while(1);
}
//activating LOX5
digitalWrite(SHT_LOX5, HIGH);
delay(10);
//initing LOX5
if(!lox5.begin(LOX5_ADDRESS)) {
Serial.println(F("Failed to boot 5 VL53L0X"));
//while(1);
}
//activating LOX6
digitalWrite(SHT_LOX6, HIGH);
delay(10);
//initing LOX6
if(!lox6.begin(LOX6_ADDRESS)) {
Serial.println(F("Failed to boot 6 VL53L0X"));
//while(1);
}
}
void read_five_sensors() {
lox1.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!
lox2.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!
lox3.rangingTest(&measure3, false); // pass in 'true' to get debug data printout!
lox4.rangingTest(&measure4, false); // pass in 'true' to get debug data printout!
lox5.rangingTest(&measure5, false); // pass in 'true' to get debug data printout!
lox6.rangingTest(&measure6, false); // pass in 'true' to get debug data printout!
// print sensor one reading
if(measure1.RangeStatus != 4 && measure1.RangeMilliMeter < 2000) { // if not out of range
radar_sx_ant_45 = measure1.RangeMilliMeter;
} else {
radar_sx_ant_45 = 2000;
}
if (diagnostic == ATTIVATO)
{
Serial.print("1: ");
Serial.print(" ");
Serial.print(radar_sx_ant_45);
Serial.print("mm");
Serial.print(" ");
}
// print sensor two reading
if(measure2.RangeStatus != 4 && measure2.RangeMilliMeter < 2000) {
radar_ant = measure2.RangeMilliMeter;
} else {
radar_ant = 2000;
}
if (diagnostic == ATTIVATO)
{
Serial.print("2: ");
Serial.print(radar_ant);
Serial.print("mm");
}
//Serial.print(" ");
///Robojax.com code see video https://youtu.be/0glBk917HPg
// print sensor three reading
if(measure3.RangeStatus != 4 && measure3.RangeMilliMeter < 2000) {
radar_dx_ant_45 = measure3.RangeMilliMeter;
} else {
radar_dx_ant_45 = 2000;
}
if (diagnostic == ATTIVATO)
{
Serial.print("3: ");
Serial.print(radar_dx_ant_45);
Serial.print("mm");
}
//Serial.print(" ");
// print sensor four reading
if(measure4.RangeStatus != 4 && measure4.RangeMilliMeter < 2000) {
radar_dx_center = measure4.RangeMilliMeter;
} else {
radar_dx_center = 2000;
}
if (diagnostic == ATTIVATO)
{
Serial.print("4: ");
Serial.print(radar_dx_center);
Serial.print("mm");
}
// print sensor 5 reading
if(measure5.RangeStatus != 4 && measure5.RangeMilliMeter < 2000) {
radar_sx_center = measure5.RangeMilliMeter;
} else {
radar_sx_center = 2000;
}
if (diagnostic == ATTIVATO)
{
Serial.print("5: ");
Serial.print(radar_sx_center);
Serial.print("mm");
Serial.println();
}
}
void setup1() {
Wire1.setSDA(2);
Wire1.setSCL(3);
Wire1.begin();
Serial.begin(9600);
pinMode(SHT_LOX1, OUTPUT);
pinMode(SHT_LOX2, OUTPUT);
pinMode(SHT_LOX3, OUTPUT);
pinMode(SHT_LOX4, OUTPUT);
pinMode(SHT_LOX5, OUTPUT);
pinMode(SHT_LOX6, OUTPUT);
pinMode(reset_button, OUTPUT);
Serial.println("Shutdown pins inited...");
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
digitalWrite(SHT_LOX3, LOW);
digitalWrite(SHT_LOX4, LOW);
digitalWrite(SHT_LOX5, LOW);
digitalWrite(SHT_LOX6, LOW);
setID();
radar_servo.attach(15);
Serial1.println();
delay(2000);
Serial1.println("Setup CORE 1 [OK]");
}
void rad_mot()
{
int pos;
for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
// in steps of 1 degree
radar_servo.write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
radar_servo.write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
}
void loop1() {
read_five_sensors();
delay(300);
//anticollision();
//rad_mot();
}
void setup() {
Serial1.setRX(1);
Serial1.setTX(0);
Serial1.begin(9600);
EEPROM.begin(512);
scanner.Init();
Wire.setSDA(4);
Wire.setSCL(5);
Wire.begin();
//EEPROM.write(10,0);
Serial1.println( F("Welcome to the SilverRover"));
Serial1.println( F("Inserisci il comando da inviare al Rover."));
Serial1.println( F("scrivi 'help' per vedere i comandi disponibili"));
pwm.begin();
pwm.setOscillatorFrequency(27000000);
pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates
panels_positions_data_load();
load_trimmer();
load_odo_eeprom();
sterzo_dritto();
ruote_sterzanti_post = DISATTIVATO;
//pca_on_off();//disattiva pca
//VoltMeter==============================
pinMode(analogInput_1, INPUT); //assigning the input port
pinMode(analogInput_2, INPUT);
//=========================================
}
void load_trimmer()
{
int trim_ = 0;
EEPROM.get(10, trim_);
if(trim_ >= -10 && trim_ <= 10)
{
trimmer = trim_;
if (diagnostic == ATTIVATO)
{
Serial1.print(F("Trimmer caricato: "));
Serial1.println(trimmer);
}
}else trimmer = 0;
}
void panels_positions_data_load()
{
int x = EEPROM.read(0);
int y = EEPROM.read(1);
//trimmer = EEPROM.read(10);
if (x == 0 || x == 1 && y == 0 || y == 1)
{
POSIZIONE_PANNELLO_DX = EEPROM.read(0);
POSIZIONE_PANNELLO_SX = EEPROM.read(1);
if(POSIZIONE_PANNELLO_DX == APERTO || POSIZIONE_PANNELLO_SX == APERTO)
{
panels_security_distance = 150;
}else if(POSIZIONE_PANNELLO_DX == CHIUSO && POSIZIONE_PANNELLO_SX == CHIUSO)
{
panels_security_distance = 0;
}
if (diagnostic == ATTIVATO)
{
Serial1.print(F("DX: "));
Serial1.println(POSIZIONE_PANNELLO_DX);
Serial1.print(F("SX: "));
Serial1.println(POSIZIONE_PANNELLO_SX);
}
}
}
void panels_positions_data_save()
{
EEPROM.write(0, POSIZIONE_PANNELLO_DX);
EEPROM.write(1, POSIZIONE_PANNELLO_SX);
if (EEPROM.commit())
{
if (diagnostic == ATTIVATO)
{
Serial1.println("EEPROM successfully committed");
}
} else {
Serial1.println("ERROR! EEPROM commit failed");
}
}
//void write_file_sd_card()
//{
// myFile = SD.open("test.txt", FILE_WRITE);
// if (myFile) {
// //Serial1.print("Writing to test.txt...");
// myFile.print(distanza_percorsa);
// // close the file:
// myFile.close();
// Serial1.println("done.");
// } else {
// // if the file didn't open, print an error:
// Serial1.println("error opening test.txt");
// }
//}
//void carica_distanza_percorsa_sd_card()
//{
// myFile = SD.open("test.txt");
// if (myFile) {
// Serial1.println("test.txt:");
//
// // read from the file until there's nothing else in it:
// while (myFile.available()) {
// Serial1.write(myFile.read());
// distanza_percorsa = myFile.read();
// }
// // close the file:
// myFile.close();
// } else {
// // if the file didn't open, print an error:
// Serial1.println("error opening test.txt");
// }
//}
//void sd_card_init() {
// //Serial1.print("Initializing SD card...");
//
// if (!SD.begin(10)) {
// Serial1.println("initialization SD Card failed!");
// while (1);
// }
// //Serial1.println("initialization done.");
//
//
// // re-open the file for reading:
//
// //sd_card_info();
//}
void pca_on_off()
{
if (pca_off == DISATTIVATO)
{
pca_off = ATTIVATO;
digitalWrite(2, HIGH);
Serial1.println(F("Motori disattivati"));
} else if (pca_off == ATTIVATO)
{
pca_off = DISATTIVATO;
digitalWrite(2, LOW);
Serial1.println(F("Motori attivati"));
}
}
void setServoPulse(uint8_t n, double pulse) {
double pulselength;
pulselength = 1000000; // 1,000,000 us per second
pulselength /= SERVO_FREQ; // Analog servos run at ~60 Hz updates
Serial1.print(pulselength); Serial1.println( F(" us per period"));
pulselength /= 4096; // 12 bits of resolution
Serial1.print(pulselength); Serial1.println( F(" us per bit"));
pulse *= 1000000; // convert input seconds to us
pulse /= pulselength;
Serial1.println(pulse);
}
void forward(uint16_t comand)
{
if (comand == STOP)
{
fine_conteggio_distanza = millis();
pwm.writeMicroseconds(4, STOP);//-30 //ANT_SX
pwm.writeMicroseconds(5, STOP);//-50 //ANT_DX
pwm.writeMicroseconds(6, STOP);//-150q //POST_DX
pwm.writeMicroseconds(7, STOP - 50);//POST_SX -> SCOLLEGATO
calcolo_distanza_percorsa();
if(avanzamento_c == ATTIVATO)
avanzamento_c = DISATTIVATO;
} else if (comand == FORWARD)
{
inizio_conteggio_distanza = millis();
pwm.writeMicroseconds(4, FORWARD - motor_speed - ant_sx_speed);//ANT_SX
pwm.writeMicroseconds(5, BACK + motor_speed + ant_dx_speed);//ANT_DX
pwm.writeMicroseconds(6, BACK + motor_speed + post_dx_speed);//POST_DX
pwm.writeMicroseconds(7, FORWARD - motor_speed - post_sx_speed - 465);//POST_SX -> SCOLLEGATO
} else if (comand == BACK)
{
inizio_conteggio_distanza = millis();
pwm.writeMicroseconds(4, BACK + motor_speed + ant_sx_speed);//ANT_SX
pwm.writeMicroseconds(5, FORWARD - motor_speed - ant_dx_speed);//ANT_DX
pwm.writeMicroseconds(6, FORWARD - motor_speed - post_dx_speed);//POST_DX
pwm.writeMicroseconds(7, BACK + motor_speed - post_sx_speed + 355);//POST_SX -> SCOLLEGATO
}
}
bool zona_radar(uint16_t value)
{
if (value > 230 && value < 400)
return true;
return false;
}
void webcam_control()
{
uint16_t webcam_sinistra_destra_l = webcam_sinistra_destra;
uint16_t webcam_sopra_sotto_l = webcam_sopra_sotto;
if (zona_radar(webcam_sinistra_destra) && webcam_sopra_sotto > 435)
webcam_sopra_sotto_l = 435;
pwm.setPWM(15, 0, webcam_sinistra_destra_l);
pwm.setPWM(14, 0, webcam_sopra_sotto_l);
}
void _webcam_sterzo(uint16_t pulselen)
{
webcam_sinistra_destra = pulselen - 60;
webcam_sopra_sotto = 315;
pwm.setPWM(15, 0, webcam_sinistra_destra);
pwm.setPWM(14, 0, webcam_sopra_sotto);
}
void sterzo_anteriore(uint16_t pulselen)
{
pulselen = pulselen + trimmer;
int p_p = map(pulselen, limite_sinistro, limite_destro, 264 , 397);
// int ant_sx_int = map(pulselen, limite_sinistro, centro, -270, 0);
// int ant_dx_int = map(pulselen -11, centro, limite_destro, 0, 270);
// int post_sx_int = map(pulselen - 11, limite_sinistro, centro, 65, 0);
// int post_sx_ext = map(pulselen - 11, centro, limite_destro, 0, 20);
// int post_dx_int = map(pulselen - 11, centro, limite_destro, 0, 380);
// int post_dx_ext = map(pulselen - 11, limite_sinistro, centro, -200, 0);
// int post_sx_int = map(pulselen - 11, 220, 330, 65, 0);
// int post_sx_ext = map(pulselen - 11, 330, 440, 0, 20);
if (pulselen > centro)
{
pwm.setPWM(0, 0, pulselen - 8); //-5
pwm.setPWM(1, 0, p_p + 68);
//============================================
ant_dx_speed = map(pulselen, centro, limite_destro, 0, 270);//ant_dx_int;
post_dx_speed = map(pulselen, centro, limite_destro, 0, 380);//post_dx_int;
post_sx_speed = map(pulselen, centro, limite_destro, 0, 20);//post_sx_ext;
if(avanzamento_c == ATTIVATO){
forward(FORWARD);
}
//============================================
} else if (pulselen < centro)
{
pwm.setPWM(0, 0, p_p - 8); //-5
pwm.setPWM(1, 0, pulselen + 68);
//============================================
ant_sx_speed = map(pulselen, limite_sinistro, centro, 270, 0);//ant_sx_int;
post_sx_speed = map(pulselen, limite_sinistro, centro, 30, 0);//post_sx_int;//post_int;
post_dx_speed = map(pulselen, limite_sinistro, centro, 200, 0);//post_dx_ext;
if(avanzamento_c == ATTIVATO){
forward(FORWARD);
}
//============================================
} else {
pwm.setPWM(0, 0, pulselen - 8); //-5
pwm.setPWM(1, 0, pulselen + 68);
ant_sx_speed = 0;
ant_dx_speed = 0;//ant_int;
post_dx_speed = 0;//post_int;
post_sx_speed = 0;//post_ext;
if(avanzamento_c == ATTIVATO){
forward(FORWARD);
}
}
if(diagnostic == ATTIVATO)
{
Serial1.print("ANT SX SPEED: ");
Serial1.println(ant_sx_speed);
Serial1.print("POST SX SPEED (Modded servo): ");
Serial1.println(post_sx_speed);
Serial1.print("ANT DX SPEED: ");
Serial1.println(ant_dx_speed);
Serial1.print("POST DX SPEED: ");
Serial1.println(post_dx_speed);
}
}
void sterzo_posteriore(uint16_t pulselen)
{
// if(webcam_sterzo == ATTIVATO)
// _webcam_sterzo(pulselen);
// if(radar_sterzo ==ATTIVATO)
// radar_motor(pulselen);
// delay(200);
pwm.setPWM(2, 0, pulselen + 38); //40
pwm.setPWM(3, 0, pulselen - 5);
}
void sterzo(uint16_t angolo_serzata)
{
uint16_t pulselen_ant = angolo_serzata;
uint16_t pulselen_post = pulselen_ant + 25;
int calc_angle = STERZO_DRITTO - pulselen_ant;
uint16_t angle_post = STERZO_DRITTO + calc_angle + 25;
if (ruote_sterzanti_ant == ATTIVATO);
sterzo_anteriore(pulselen_ant);
if (ruote_sterzanti_post == ATTIVATO)
sterzo_posteriore(angle_post);
if (webcam_sterzo == ATTIVATO)
_webcam_sterzo(angle_post);
if (radar_sterzo == ATTIVATO)
radar_motor(angle_post);
//delay(200);
}
void sterzo_dritto()
{
if (posizione_sterzo != DRITTO)
{
if (posizione_sterzo < DRITTO) {
for (uint16_t i = posizione_sterzo; i <= DRITTO; i++)
{
posizione_sterzo = i;
sterzo(posizione_sterzo);
delay(2);
}
} else if (posizione_sterzo > DRITTO)
{
for (uint16_t i = posizione_sterzo; i >= DRITTO; i--)
{
posizione_sterzo = i;
sterzo(posizione_sterzo);
delay(2);
}
}
}
posizione_sterzo = DRITTO;
sterzo(STERZO_DRITTO);
}
void panel_dx_da_meta_a_chiuso()
{
if (POSIZIONE_PANNELLO_DX == META)
{
for (uint16_t i = 300; i < 520; i++)
{
pwm.setPWM(8, 0, i/*i*/);
panel_position_dx = i;
pwm.setPWM(9, 0, 328 - i/*i*/);
delay(panel_speed);
}
pwm.setPWM(8, 0, 0/*i*/);
POSIZIONE_PANNELLO_DX = 0;
}
}
void panel_dx_da_chiuso_a_meta()
{
if (POSIZIONE_PANNELLO_DX == 0)
{
for (uint16_t i = 520; i > 300; i--) {
pwm.setPWM(8, 0, i/*i*/);
panel_position_dx = i;
pwm.setPWM(9, 0, i/*i*/);
delay(panel_speed);
}
pwm.setPWM(8, 0, 0/*i*/);
POSIZIONE_PANNELLO_DX = META;
}
}
void panel_dx_da_meta_a_aperto()
{
if (POSIZIONE_PANNELLO_DX == META)
{
for (uint16_t i = 300; i > 90; i--)
{
pwm.setPWM(8, 0, i/*i*/);
panel_position_dx = i;
pwm.setPWM(9, 0, i/*i*/);
delay(panel_speed);
}
pwm.setPWM(8, 0, 0/*i*/);
POSIZIONE_PANNELLO_DX = APERTO ;
}
}
void panel_dx_da_aperto_a_meta()
{
if (POSIZIONE_PANNELLO_DX == APERTO )
{
for (uint16_t i = 90; i < 300; i++)
{
pwm.setPWM(8, 0, i/*i*/);
panel_position_dx = i;
pwm.setPWM(9, 0, i/*i*/);
delay(panel_speed);
}
pwm.setPWM(8, 0, 0/*i*/);
POSIZIONE_PANNELLO_DX = META;
}
}
void panel_sx_da_meta_a_chiuso()
{
if (POSIZIONE_PANNELLO_SX == META)
{
for (uint16_t i = 450; i > 190; i--)
{
//pwm.setPWM(10, 0, 328 + i/*i*/);
pwm.setPWM(10, 0, i/*i*/);
panel_position_sx = i;
delay(panel_speed);
}
pwm.setPWM(10, 0, 0/*328+i/*i*/);
POSIZIONE_PANNELLO_SX = 0;
}
}
void panel_sx_da_chiuso_a_meta()
{
if (POSIZIONE_PANNELLO_SX == 0)
{
for (uint16_t i = 190; i < 450; i++)
{
pwm.setPWM(10, 0, i/*328+i/*i*/);
panel_position_sx = i;
//pwm.setPWM(11, 0, i/*328-i/*i*/);
delay(panel_speed);
}
POSIZIONE_PANNELLO_SX = META;
}
}
void panel_sx_da_meta_a_aperto()
{
if (POSIZIONE_PANNELLO_SX == META)
{
for (uint16_t i = 450; i < 640; i++/*int i=0;i<240;i++*/)
{
pwm.setPWM(10, 0, i/*328+i/*i*/);
panel_position_sx = i;
//pwm.setPWM(11, 0, i/*328+i/*i*/);
delay(panel_speed);
}
}
pwm.setPWM(10, 0, 0/*328+i/*i*/);
POSIZIONE_PANNELLO_SX = APERTO ;
}
void panel_sx_da_aperto_a_meta()
{
if (POSIZIONE_PANNELLO_SX == APERTO )
{
for (uint16_t i = 640; i > 450; i--)
{
//pwm.setPWM(10, 0, 328 - i/*i*/);
pwm.setPWM(10, 0, i/*i*/);
panel_position_sx = i;
delay(panel_speed);
}
POSIZIONE_PANNELLO_SX = META;
}
}
void spegni_motori_pannelli()
{
pwm.setPWM(8, 0, 0/*i*/);
pwm.setPWM(10, 0, 0/*i*/);
}
void panel_sx_dx_da_meta_a_aperto()
{
if (POSIZIONE_PANNELLO_SX == META && POSIZIONE_PANNELLO_DX == META)
{
//panel_position_dx = 300;
//panel_position_sx = 450;
for (uint8_t i = 0; i < 210; i++)
{
if (panel_position_dx > 90)
{
pwm.setPWM(8, 0, 300 - i/*i*/);
panel_position_dx = 300 - i;
}
if (panel_position_sx < 640)
{
pwm.setPWM(10, 0, 430 + i/*i*/);
panel_position_sx = 430 + i;
}
delay(panel_speed);
}
spegni_motori_pannelli();
POSIZIONE_PANNELLO_SX = APERTO ;
POSIZIONE_PANNELLO_DX = APERTO ;
}
}
void panel_sx_dx_da_apero_a_meta()
{
if (POSIZIONE_PANNELLO_SX == APERTO && POSIZIONE_PANNELLO_DX == APERTO )
{
for (uint8_t i = 210; i > 0; i--)
{
if (panel_position_dx < 300)
{
pwm.setPWM(8, 0, 300 - i/*i*/);
panel_position_dx = 300 - i;
}
if (panel_position_sx > 449)
{
pwm.setPWM(10, 0, 450 + i/*i*/);
panel_position_sx = 450 + i;
}
delay(panel_speed);
}
POSIZIONE_PANNELLO_SX = META;
POSIZIONE_PANNELLO_DX = META;
}
}
void open_panels()
{
if (POSIZIONE_PANNELLO_DX == CHIUSO && POSIZIONE_PANNELLO_SX == CHIUSO && peso_sinistra == DISATTIVATO && peso_destra == DISATTIVATO)
{
panel_dx_da_chiuso_a_meta();
Serial1.println( F("Pannello destro aperto a metà"));
panel_sx_da_chiuso_a_meta();
Serial1.println( F("Pannello sinistro aperto a metà"));
panel_sx_dx_da_meta_a_aperto();
// POSIZIONE_PANNELLO_SX = 0;
Serial1.println( F("Entrambi i pannelli aperti"));
POSIZIONE_PANNELLO_DX = APERTO ;
POSIZIONE_PANNELLO_SX = APERTO ;
stato_precedente = APERTO ;
} else if (peso_sinistra == ATTIVATO)
{
stato_precedente = 1;
peso_a_sinistra();
} else if (peso_destra == ATTIVATO)
{
stato_precedente = 1;
peso_a_destra();
}
panels_positions_data_save();
//Serial1.println( F("azioni Pannello sx effettuate"));
}
void close_panels()
{
if (POSIZIONE_PANNELLO_DX == APERTO && POSIZIONE_PANNELLO_SX == APERTO && peso_sinistra == DISATTIVATO && peso_destra == DISATTIVATO)
{
panel_sx_dx_da_apero_a_meta();
Serial1.println( F("Pannello sinistro aperto a metà"));
Serial1.println( F("Pannello destro aperto a metà"));
panel_sx_da_meta_a_chiuso();
panel_dx_da_meta_a_chiuso();
// POSIZIONE_PANNELLO_SX = 0;
Serial1.println( F("Entrambi i pannelli chiusi"));
Serial1.println( F("azioni Pannello sx effettuate"));
POSIZIONE_PANNELLO_DX = 0;
POSIZIONE_PANNELLO_SX = 0;
stato_precedente = 0;
} else if (peso_sinistra == ATTIVATO)
{
stato_precedente = 0;
peso_a_sinistra();
} else if (peso_destra == ATTIVATO)
{
stato_precedente = 0;
peso_a_destra();
}
panels_positions_data_save();
}
void left_panel()
{
if (POSIZIONE_PANNELLO_SX == 0)
{
panel_sx_da_chiuso_a_meta();
Serial1.println( F("Pannello sinistro aperto a metà"));
panel_sx_da_meta_a_aperto();
//POSIZIONE_PANNELLO_SX = APERTO ;
Serial1.println( F("Pannello sinistro aperto completamente"));
} else if (POSIZIONE_PANNELLO_SX == APERTO )
{
panel_sx_da_aperto_a_meta();
Serial1.println( F("Pannello sinistro aperto a metà"));
//panel_dx_da_chiuso_a_meta();
panel_sx_da_meta_a_chiuso();
//POSIZIONE_PANNELLO_SX = 0;
if (POSIZIONE_PANNELLO_SX == 0)
Serial1.println( F("Pannello sinistro 0"));
} else if (POSIZIONE_PANNELLO_SX == 2)
{
panel_sx_da_meta_a_chiuso();
//POSIZIONE_PANNELLO_SX = 0;
if (POSIZIONE_PANNELLO_SX == 0)