|
@@ -1,7 +1,7 @@
|
1
|
1
|
# 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
2
|
2
|
# 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
3
|
3
|
//Verfassser: Felix Stange, 4056379, MET2016
|
4
|
|
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 13.04.2018
|
|
4
|
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 04.05.2018
|
5
|
5
|
//Projektbeschreibung: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe
|
6
|
6
|
//der Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen).
|
7
|
7
|
//Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es
|
|
@@ -58,6 +58,7 @@ void BlueSetup()
|
58
|
58
|
Serial1.begin(9600); //Initialisierung mit Datengeschwindigkeit(Baud)
|
59
|
59
|
Serial1.setTimeout(10); //verkürzt Serial(1).read auf 10 ms statt 1000 ms
|
60
|
60
|
if(Serial1.available()) Serial.println("Bluetoothverbindung hergestellt");
|
|
61
|
+ if(Serial1.available() > 0) Serial1.read(); //Verwerfen der alten Informationen aus dem Puffer
|
61
|
62
|
}
|
62
|
63
|
|
63
|
64
|
//Setup Liniensensoren
|
|
@@ -66,27 +67,27 @@ void LineSetup()
|
66
|
67
|
ledYellow(1);
|
67
|
68
|
lineSensors.initThreeSensors(); //Initialisierung von 3 Sensoren (max 5)
|
68
|
69
|
|
69
|
|
- // //Kalibrierung
|
70
|
|
- // uint32_t total[3] = {0, 0, 0};
|
71
|
|
- // for(uint8_t i = 0; i < 120; i++)
|
72
|
|
- // {
|
73
|
|
- // if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEEDLEFT, -FULLSPEEDRIGHT);
|
74
|
|
- // else motors.setSpeeds(-FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
75
|
|
- // lineSensors.read(lineOffset);
|
76
|
|
- // total[0] += lineOffset[0];
|
77
|
|
- // total[1] += lineOffset[1];
|
78
|
|
- // total[2] += lineOffset[2];
|
79
|
|
- lineSensors.calibrate();
|
80
|
|
- // }
|
81
|
|
- // motors.setSpeeds(0, 0);
|
82
|
|
- // lineOffset[0] = total[0] / 120;
|
83
|
|
- // lineOffset[1] = total[1] / 120;
|
84
|
|
- // lineOffset[2] = total[2] / 120;
|
|
70
|
+ //Kalibrierung
|
|
71
|
+ uint32_t total[3] = {0, 0, 0};
|
|
72
|
+ for(uint8_t i = 0; i < 120; i++)
|
|
73
|
+ {
|
|
74
|
+ if (i > 30 && i <= 90) motors.setSpeeds(106, -100);
|
|
75
|
+ else motors.setSpeeds(-106, 100);
|
|
76
|
+ lineSensors.read(lineOffset);
|
|
77
|
+ total[0] += lineOffset[0];
|
|
78
|
+ total[1] += lineOffset[1];
|
|
79
|
+ total[2] += lineOffset[2];
|
|
80
|
+ lineSensors.calibrate();
|
|
81
|
+ }
|
|
82
|
+ motors.setSpeeds(0, 0);
|
|
83
|
+ lineOffset[0] = total[0] / 120;
|
|
84
|
+ lineOffset[1] = total[1] / 120;
|
|
85
|
+ lineOffset[2] = total[2] / 120;
|
85
|
86
|
ledYellow(0);
|
86
|
87
|
|
87
|
|
- lineOffset[0] = 240;
|
88
|
|
- lineOffset[1] = 120;
|
89
|
|
- lineOffset[2] = 220;
|
|
88
|
+ // lineOffset[0] = 240;
|
|
89
|
+ // lineOffset[1] = 120;
|
|
90
|
+ // lineOffset[2] = 220;
|
90
|
91
|
}
|
91
|
92
|
|
92
|
93
|
//Setup Abstandssensoren
|
|
@@ -106,8 +107,8 @@ void GyroSetup()
|
106
|
107
|
ledRed(1);
|
107
|
108
|
while(1)
|
108
|
109
|
{
|
109
|
|
- //Serial1.println("Fehler Drehbewegungssensors");
|
110
|
|
- delay(1000);
|
|
110
|
+ Serial.println("Fehler Drehbewegungssensors");
|
|
111
|
+ delay(5000);
|
111
|
112
|
}
|
112
|
113
|
}
|
113
|
114
|
gyro.writeReg(L3G::CTRL1, 0b11111111); //Ausgaberate 800Hz, Tiefpassfilter 100Hz
|
|
@@ -139,8 +140,8 @@ void CompassSetup()
|
139
|
140
|
ledRed(1);
|
140
|
141
|
while(1)
|
141
|
142
|
{
|
142
|
|
- //Serial1.println("Fehler Beschleunigungssensors");
|
143
|
|
- delay(1000);
|
|
143
|
+ Serial.println("Fehler Beschleunigungssensors");
|
|
144
|
+ delay(5000);
|
144
|
145
|
}
|
145
|
146
|
}
|
146
|
147
|
compass.enableDefault();
|
|
@@ -296,27 +297,26 @@ void EncoderUpdate()
|
296
|
297
|
void Kontaktvermeiden()
|
297
|
298
|
{
|
298
|
299
|
//Serial1.println("Kontaktvermeiden");
|
299
|
|
- Serial1.println(1);
|
|
300
|
+ Serial1.print(1);
|
300
|
301
|
ledRed(1);
|
301
|
|
-
|
302
|
302
|
motors.setSpeeds(0, 0);
|
303
|
|
- delay(500);
|
|
303
|
+ delay(1000);
|
304
|
304
|
while(proxValue[1] == 0 || proxValue[2] == 0)
|
305
|
305
|
{
|
306
|
306
|
ProxUpdate();
|
307
|
|
- motors.setSpeeds(-54, -50);
|
308
|
|
- if(lineValue[0] > 150 || lineValue[2] > 170) break;
|
|
307
|
+ motors.setSpeeds(-53, -50);
|
|
308
|
+ if(lineValue[0] > 150 || lineValue[2] > 120) break;
|
309
|
309
|
}
|
310
|
|
- delay(500);
|
|
310
|
+ motors.setSpeeds(0, 0);
|
311
|
311
|
//Serial1.println("Vermeiden beendet");
|
312
|
|
- Serial1.println(0);
|
|
312
|
+ Serial1.print(0);
|
313
|
313
|
}
|
314
|
314
|
|
315
|
315
|
//Funktion Hindernisumfahrung
|
316
|
316
|
void Hindernisumfahren()
|
317
|
317
|
{
|
318
|
318
|
//Serial1.println("Hindernisumfahren");
|
319
|
|
- Serial1.println(1);
|
|
319
|
+ Serial1.print(1);
|
320
|
320
|
ledYellow(1);
|
321
|
321
|
|
322
|
322
|
//Schritt 1: Spurwechsel links
|
|
@@ -329,14 +329,14 @@ void Hindernisumfahren()
|
329
|
329
|
GyroUpdate();
|
330
|
330
|
LineUpdate();
|
331
|
331
|
motors.setSpeeds(27, 100);
|
332
|
|
- if(lineValue[2] > 170 && lineValue[2] < 450) break;
|
|
332
|
+ if(lineValue[2] > 120 && lineValue[2] < 500) break;
|
333
|
333
|
}
|
334
|
334
|
//geradeaus über Mittellinie fahren
|
335
|
335
|
LineUpdate();
|
336
|
|
- while(lineValue[2] < 170)
|
|
336
|
+ while(lineValue[2] < 120)
|
337
|
337
|
{
|
338
|
338
|
LineUpdate();
|
339
|
|
- motors.setSpeeds(107, 100);
|
|
339
|
+ motors.setSpeeds(106, 100);
|
340
|
340
|
//if(lineValue[0] > MARKLINE0) break;
|
341
|
341
|
}
|
342
|
342
|
//rechts drehen
|
|
@@ -345,12 +345,12 @@ void Hindernisumfahren()
|
345
|
345
|
{
|
346
|
346
|
GyroUpdate();
|
347
|
347
|
LineUpdate();
|
348
|
|
- if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 0);
|
349
|
|
- else if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(107, 100);
|
350
|
|
- else motors.setSpeeds(107, 25);
|
|
348
|
+ if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(106, 0);
|
|
349
|
+ else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(106, 100);
|
|
350
|
+ else motors.setSpeeds(106, 25);
|
351
|
351
|
}
|
352
|
352
|
//geradeaus fahren
|
353
|
|
- motors.setSpeeds(107, 100);
|
|
353
|
+ motors.setSpeeds(106, 100);
|
354
|
354
|
|
355
|
355
|
//Gegenverkehr beachten
|
356
|
356
|
ProxUpdate();
|
|
@@ -395,15 +395,15 @@ void Hindernisumfahren()
|
395
|
395
|
{
|
396
|
396
|
GyroUpdate();
|
397
|
397
|
LineUpdate();
|
398
|
|
- motors.setSpeeds(107, 25);
|
399
|
|
- if(lineValue[0] > 150 && lineValue[0] < 400) break;
|
|
398
|
+ motors.setSpeeds(106, 25);
|
|
399
|
+ if(lineValue[0] > 150 && lineValue[0] < 500) break;
|
400
|
400
|
}
|
401
|
401
|
//geradeaus über Mittellinie fahren
|
402
|
402
|
LineUpdate();
|
403
|
403
|
while(lineValue[0] < 150)
|
404
|
404
|
{
|
405
|
405
|
LineUpdate();
|
406
|
|
- motors.setSpeeds(107, 100);
|
|
406
|
+ motors.setSpeeds(106, 100);
|
407
|
407
|
//if(lineValue[0] > MARKLINE0) break;
|
408
|
408
|
}
|
409
|
409
|
//links drehen
|
|
@@ -412,14 +412,14 @@ void Hindernisumfahren()
|
412
|
412
|
{
|
413
|
413
|
GyroUpdate();
|
414
|
414
|
LineUpdate();
|
415
|
|
- if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(0, 100);
|
416
|
|
- else if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 100);
|
|
415
|
+ if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(0, 100);
|
|
416
|
+ else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(106, 100);
|
417
|
417
|
else motors.setSpeeds(27, 100);
|
418
|
418
|
}
|
419
|
419
|
//geradeaus fahren
|
420
|
420
|
//Serial1.println("Umfahren beendet");
|
421
|
|
- Serial1.println(0);
|
422
|
|
- motors.setSpeeds(107, 100);
|
|
421
|
+ Serial1.print(0);
|
|
422
|
+ motors.setSpeeds(106, 100);
|
423
|
423
|
}
|
424
|
424
|
|
425
|
425
|
//Funktion Abbiegen
|
|
@@ -427,70 +427,65 @@ void Abbiegen()
|
427
|
427
|
{
|
428
|
428
|
ledYellow(1);
|
429
|
429
|
//Serial1.println("Abbiegen");
|
430
|
|
- Serial1.println(1);
|
|
430
|
+ Serial1.print(1);
|
431
|
431
|
|
432
|
432
|
//Markierung analysieren
|
433
|
433
|
LineUpdate();
|
434
|
434
|
bool leftCode; //links => 1
|
435
|
435
|
bool rightCode; //rechts => 2
|
436
|
|
- if(lineValue[0] > 400) leftCode = true;
|
|
436
|
+ if((lineValue[0] > 500 && lineValue[0] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) leftCode = true;
|
437
|
437
|
else leftCode = false;
|
438
|
|
- if(lineValue[2] > 450) rightCode = true;
|
|
438
|
+ if((lineValue[2] > 500 && lineValue[2] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) rightCode = true;
|
439
|
439
|
else rightCode = false;
|
440
|
440
|
|
441
|
441
|
//Zufallszahl erzeugen
|
442
|
442
|
uint8_t randy; //geradeaus => 0
|
443
|
443
|
if(leftCode == true && rightCode == true) randy = random(1, 3); //1, 2
|
444
|
|
- else if(leftCode == true && rightCode == false) randy = 1;
|
445
|
|
- // else if(leftCode == true && rightCode == false)
|
446
|
|
- // {
|
447
|
|
- // randy = random(0, 2); //0, 1
|
448
|
|
- // if(randy == 0) randy = random(0, 2); //erhöht Wahrscheinlickeit abzubiegen
|
449
|
|
- // }
|
450
|
|
- else if(leftCode == false && rightCode == true) randy = 2;
|
451
|
|
- // else if(leftCode == false && rightCode == true)
|
452
|
|
- // {
|
453
|
|
- // randy = random(3); //0, (1), 2
|
454
|
|
- // if(randy == 0) randy = random(3); //erhöht Wahrscheinlickeit abzubiegen
|
455
|
|
- // while(randy == 1) randy = random(3); //!1 => 0, 2
|
456
|
|
- // }
|
|
444
|
+ else if(leftCode == true && rightCode == false) //randy = 1;
|
|
445
|
+ {
|
|
446
|
+ randy = random(0, 2); //0, 1
|
|
447
|
+ //if(randy == 0) randy = random(0, 2); //erhöht Wahrscheinlickeit abzubiegen
|
|
448
|
+ }
|
|
449
|
+ else if(leftCode == false && rightCode == true) //randy = 2;
|
|
450
|
+ {
|
|
451
|
+ randy = random(0, 2); //0, 1
|
|
452
|
+ //if(randy == 0) randy = random(0, 2); //erhöht Wahrscheinlickeit abzubiegen
|
|
453
|
+ if(randy == 1) randy = 2; //!1 => 2
|
|
454
|
+ }
|
457
|
455
|
|
458
|
456
|
//links Abbiegen (von der Verbindungsstrecke)
|
459
|
457
|
if(randy == 1 && rightCode == true)
|
460
|
458
|
{
|
461
|
459
|
//Serial1.println("links Abbiegen von der Verbindungsstrecke");
|
462
|
460
|
//geradeaus zur Mittellinie fahren
|
463
|
|
- motors.setSpeeds(107, 100 +10);
|
|
461
|
+ motors.setSpeeds(106, 100 +10);
|
464
|
462
|
lastUpdate = millis();
|
465
|
463
|
TimeUpdate();
|
466
|
464
|
while(eventTime < 300)
|
467
|
465
|
{
|
468
|
466
|
TimeUpdate();
|
469
|
|
- //Serial1.println(eventTime);
|
470
|
|
- motors.setSpeeds(107, 100 +10);
|
|
467
|
+ motors.setSpeeds(106, 100 +10);
|
471
|
468
|
}
|
472
|
469
|
LineUpdate();
|
473
|
|
- //Serial1.println("Linie suchen");
|
474
|
|
- while(lineValue[0] < 150 && lineValue[2] < 170)
|
|
470
|
+ while(lineValue[0] < 150 && lineValue[2] < 120)
|
475
|
471
|
{
|
476
|
472
|
LineUpdate();
|
477
|
|
- motors.setSpeeds(107, 100);
|
|
473
|
+ motors.setSpeeds(106, 100);
|
478
|
474
|
}
|
479
|
475
|
//links drehen
|
480
|
476
|
turnAngle = 0;
|
481
|
477
|
rotationAngle = 0;
|
482
|
478
|
GyroUpdate();
|
483
|
|
- //Serial1.println("Drehen");
|
484
|
479
|
while(rotationAngle < 90)
|
485
|
480
|
{
|
486
|
481
|
GyroUpdate();
|
487
|
482
|
LineUpdate();
|
488
|
|
- if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(0, 100);
|
489
|
|
- else if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 100);
|
|
483
|
+ if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(0, 100);
|
|
484
|
+ else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(53, 100);
|
490
|
485
|
else motors.setSpeeds(27, 100);
|
491
|
486
|
}
|
492
|
487
|
//geradeaus fahren
|
493
|
|
- motors.setSpeeds(107, 100);
|
|
488
|
+ motors.setSpeeds(106, 100);
|
494
|
489
|
}
|
495
|
490
|
|
496
|
491
|
//links Abbiegen (vom Rundkurs)
|
|
@@ -507,7 +502,7 @@ void Abbiegen()
|
507
|
502
|
GyroUpdate();
|
508
|
503
|
EncoderUpdate();
|
509
|
504
|
motors.setSpeeds(27, 100);
|
510
|
|
- if(driveRotation[1] > 1) break;
|
|
505
|
+ //if(driveRotation[1] > 1) break;
|
511
|
506
|
}
|
512
|
507
|
driveRotation[1] = 0;
|
513
|
508
|
GyroUpdate();
|
|
@@ -516,23 +511,32 @@ void Abbiegen()
|
516
|
511
|
GyroUpdate();
|
517
|
512
|
EncoderUpdate();
|
518
|
513
|
motors.setSpeeds(27, 100);
|
519
|
|
- if(driveRotation[1] > 1) break;
|
|
514
|
+ //if(driveRotation[1] > 1) break;
|
520
|
515
|
}
|
521
|
516
|
//geradeaus fahren
|
522
|
|
- motors.setSpeeds(107, 100);
|
|
517
|
+ motors.setSpeeds(106, 100);
|
523
|
518
|
lastUpdate = millis();
|
524
|
519
|
TimeUpdate();
|
525
|
520
|
while(eventTime < 1000)
|
526
|
521
|
{
|
527
|
522
|
TimeUpdate();
|
528
|
523
|
LineUpdate();
|
529
|
|
- if(lineValue[2] > 170 && lineValue[2] < 450) break;
|
|
524
|
+ if(lineValue[2] > 120 && lineValue[2] < 500) break;
|
|
525
|
+ }
|
|
526
|
+ lastUpdate = millis();
|
|
527
|
+ TimeUpdate();
|
|
528
|
+ while(eventTime < 1000)
|
|
529
|
+ {
|
|
530
|
+ TimeUpdate();
|
|
531
|
+ LineUpdate();
|
|
532
|
+ Spurhalten();
|
530
|
533
|
}
|
531
|
534
|
}
|
532
|
535
|
|
533
|
536
|
//rechts Abbiegen
|
534
|
537
|
else if(randy == 2 && rightCode == true)
|
535
|
538
|
{
|
|
539
|
+ //Serial1.println("rechts Abbiegen");
|
536
|
540
|
//rechts drehen
|
537
|
541
|
turnAngle = 0;
|
538
|
542
|
rotationAngle = 0;
|
|
@@ -543,8 +547,8 @@ void Abbiegen()
|
543
|
547
|
GyroUpdate();
|
544
|
548
|
EncoderUpdate();
|
545
|
549
|
LineUpdate();
|
546
|
|
- motors.setSpeeds(107, 25);
|
547
|
|
- if(driveRotation[0] > 1 || (lineValue[0] > 150 && lineValue[0] < 400)) break;
|
|
550
|
+ motors.setSpeeds(106, 25);
|
|
551
|
+ if(lineValue[0] > 150 && lineValue[0] < 500) break;
|
548
|
552
|
}
|
549
|
553
|
GyroUpdate();
|
550
|
554
|
lastUpdate = millis();
|
|
@@ -554,17 +558,18 @@ void Abbiegen()
|
554
|
558
|
GyroUpdate();
|
555
|
559
|
LineUpdate();
|
556
|
560
|
if(eventTime > 3000) break;
|
557
|
|
- if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 0);
|
|
561
|
+ if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(106, 0);
|
558
|
562
|
//else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) motors.setSpeeds(-SLOWSPEEDLEFT, -SLOWSPEEDRIGHT);
|
559
|
|
- else if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(107, 50);
|
560
|
|
- else motors.setSpeeds(107, 25);
|
|
563
|
+ else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(106, 50);
|
|
564
|
+ else motors.setSpeeds(106, 25);
|
561
|
565
|
}
|
562
|
566
|
}
|
563
|
567
|
|
564
|
568
|
//nicht Abbiegen, geradeaus fahren
|
565
|
569
|
else
|
566
|
570
|
{
|
567
|
|
- motors.setSpeeds(107, 100);
|
|
571
|
+ //Serial1.println("nicht Abbiegen");
|
|
572
|
+ motors.setSpeeds(106, 100);
|
568
|
573
|
lastUpdate = millis();
|
569
|
574
|
TimeUpdate();
|
570
|
575
|
while(eventTime < 1000)
|
|
@@ -575,57 +580,70 @@ void Abbiegen()
|
575
|
580
|
}
|
576
|
581
|
}
|
577
|
582
|
//Serial1.println("Abbiegen beendet");
|
578
|
|
- Serial1.println(0);
|
|
583
|
+ Serial1.print(0);
|
579
|
584
|
}
|
580
|
585
|
|
581
|
586
|
//Funktion Spurhalten
|
582
|
587
|
void Spurhalten()
|
583
|
588
|
{
|
584
|
589
|
//linke Linie erreicht, nach rechts fahren
|
585
|
|
- if(lineValue[0] > 150 && lineValue[0] < 400)
|
|
590
|
+ if(lineValue[0] > 150 && lineValue[0] < 500)
|
586
|
591
|
{
|
587
|
592
|
ledYellow(1);
|
588
|
593
|
//Serial1.println("Spur nach rechts korrigieren");
|
589
|
|
- motors.setSpeeds(107/eventSpeed, 25/eventSpeed);
|
590
|
|
- delay(100);
|
|
594
|
+ motors.setSpeeds(106/eventSpeed, 25/eventSpeed);
|
|
595
|
+ lastUpdate = millis();
|
|
596
|
+ TimeUpdate();
|
|
597
|
+ while(eventTime < 100)
|
|
598
|
+ {
|
|
599
|
+ TimeUpdate();
|
|
600
|
+ LineUpdate();
|
|
601
|
+ if(lineValue[2] > 120) break;
|
|
602
|
+ }
|
591
|
603
|
}
|
592
|
604
|
|
593
|
605
|
//rechte Linie erreicht, nach links fahren
|
594
|
|
- else if(lineValue[2] > 170 && lineValue[2] < 450)
|
|
606
|
+ else if(lineValue[2] > 120 && lineValue[2] < 500)
|
595
|
607
|
{
|
596
|
608
|
ledYellow(1);
|
597
|
609
|
//Serial1.println("Spur nach links korrigieren");
|
598
|
610
|
motors.setSpeeds(27/eventSpeed, 100/eventSpeed);
|
599
|
|
- delay(100);
|
600
|
|
- }
|
601
|
|
-
|
602
|
|
- //Linie überfahren, zurücksetzen
|
603
|
|
- else if(lineValue[1] > 100 && lineValue[1] < 350)
|
604
|
|
- {
|
605
|
|
- ledRed(1);
|
606
|
|
- //Serial1.println("Spurfinden");
|
607
|
|
- Serial1.println(1);
|
608
|
611
|
lastUpdate = millis();
|
609
|
|
- while(lineValue[0] < 150 && lineValue[2] < 170) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
|
|
612
|
+ TimeUpdate();
|
|
613
|
+ while(eventTime < 100)
|
610
|
614
|
{
|
611
|
615
|
TimeUpdate();
|
612
|
616
|
LineUpdate();
|
613
|
|
- motors.setSpeeds(-107/eventSpeed, -100/eventSpeed);
|
614
|
|
- if(eventTime > 3000) break;
|
|
617
|
+ if(lineValue[0] > 150) break;
|
615
|
618
|
}
|
616
|
|
- delay(100);
|
617
|
|
- //Serial1.println("Spur gefunden");
|
618
|
|
- Serial1.println(0);
|
619
|
619
|
}
|
620
|
620
|
|
621
|
621
|
//normale Fahrt
|
622
|
622
|
else
|
623
|
623
|
{
|
624
|
624
|
ledGreen(1);
|
625
|
|
- motors.setSpeeds(107/eventSpeed, 100/eventSpeed);
|
|
625
|
+ motors.setSpeeds(106/eventSpeed, 100/eventSpeed);
|
626
|
626
|
}
|
627
|
627
|
}
|
628
|
628
|
|
|
629
|
+//Funktion Spurfinden
|
|
630
|
+void Spurfinden()
|
|
631
|
+{
|
|
632
|
+ ledRed(1);
|
|
633
|
+ //Serial1.println("Spurfinden");
|
|
634
|
+ Serial1.print(1);
|
|
635
|
+ lastUpdate = millis();
|
|
636
|
+ while(lineValue[0] < 150 && lineValue[2] < 120) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
|
|
637
|
+ {
|
|
638
|
+ TimeUpdate();
|
|
639
|
+ LineUpdate();
|
|
640
|
+ motors.setSpeeds(-106, -100);
|
|
641
|
+ if(eventTime > 3000) break;
|
|
642
|
+ }
|
|
643
|
+ //Serial1.println("Spur gefunden");
|
|
644
|
+ Serial1.print(0);
|
|
645
|
+}
|
|
646
|
+
|
629
|
647
|
//Funktion Serielle Ausgabe
|
630
|
648
|
void SerielleAusgabe()
|
631
|
649
|
{
|
|
@@ -644,17 +662,17 @@ void SerielleAusgabe()
|
644
|
662
|
|
645
|
663
|
void loop()
|
646
|
664
|
{
|
647
|
|
- //LoopTiming(); //Zykluszeit beträgt max. 30ms im Idle
|
|
665
|
+ //LoopTiming(); //Zykluszeit beträgt max. 20ms im Idle
|
648
|
666
|
ledGreen(0);
|
649
|
667
|
ledYellow(0);
|
650
|
668
|
ledRed(0);
|
651
|
669
|
|
652
|
|
- //Abfragen der Sensordaten
|
653
|
|
- LineUpdate();
|
|
670
|
+ //Abfragen der Sensordaten
|
654
|
671
|
ProxUpdate();
|
655
|
672
|
EncoderUpdate();
|
656
|
673
|
GyroUpdate();
|
657
|
674
|
CompassUpdate();
|
|
675
|
+ LineUpdate();
|
658
|
676
|
TimeUpdate();
|
659
|
677
|
|
660
|
678
|
//Funktionsauswahl
|
|
@@ -667,18 +685,20 @@ void loop()
|
667
|
685
|
if(alarm == true)
|
668
|
686
|
{
|
669
|
687
|
//Serial1.println("Verstanden");
|
670
|
|
- eventSpeed = 2;
|
671
|
|
- if(moveRate > 1000 || proxValue[1] == 6 || proxValue[2] == 6 || lineValue[0] > 400 || lineValue[2] > 450) motors.setSpeeds(0, 0);
|
|
688
|
+ eventSpeed = 1.4;
|
|
689
|
+ if(moveRate > 1500 || (proxValue[1] == 6 && proxValue[2] == 6) || lineValue[0] > 500 || lineValue[2] > 500 || lineValue[1] > 100) motors.setSpeeds(0, 0);
|
672
|
690
|
else Spurhalten();
|
673
|
691
|
}
|
674
|
692
|
//verfügbare Funktionen im Normalfall
|
675
|
693
|
else
|
676
|
694
|
{
|
677
|
|
- eventSpeed = 1;
|
|
695
|
+ eventSpeed = 1.0;
|
678
|
696
|
//if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();
|
679
|
|
- if(proxValue[2] == 6) Hindernisumfahren();
|
680
|
|
- else if(lineValue[0] > 400 && lineValue[0] < 1000 || lineValue[2] > 450 && lineValue[2] < 1000) Abbiegen();
|
|
697
|
+ if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
|
|
698
|
+ else if((lineValue[0] > 500 && lineValue[0] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) Abbiegen();
|
|
699
|
+ else if((lineValue[2] > 500 && lineValue[2] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) Abbiegen();
|
|
700
|
+ else if(lineValue[1] > 100 && lineValue[1] < 300) Spurfinden();
|
681
|
701
|
else Spurhalten();
|
682
|
702
|
}
|
683
|
|
- //LoopTiming();
|
|
703
|
+ //LoopTiming();
|
684
|
704
|
}
|