|
@@ -1,19 +1,29 @@
|
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 04.05.2018
|
5
|
|
-//Projektbeschreibung: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe
|
6
|
|
-//der Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen).
|
7
|
|
-//Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es
|
8
|
|
-//dennoch zu einer Kollision, wird diese durch den Beschleunigungssensor (LSM303) erkannt.
|
9
|
|
-//Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G)
|
10
|
|
-//genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden.
|
11
|
|
-//Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die
|
12
|
|
-//Kommunikation erfolgt seriell ('SERIAL' für USB, 'SERIAL1' für Bluetooth).
|
13
|
|
-//Das LCD kann bei Bluetoothnutzung nicht verwendet werden.
|
14
|
|
-
|
15
|
|
-# 14 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino" 2
|
|
4
|
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 08.05.2018
|
|
5
|
+//Projektthema: Untersuchungen und Implementierung eines automatisierten Fahrens mittels Pololu Zumo
|
|
6
|
+/*Kurzbeschreibung: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe
|
|
7
|
+
|
|
8
|
+der Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen).
|
|
9
|
+
|
|
10
|
+Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es
|
|
11
|
+
|
|
12
|
+dennoch zu einer Kollision, wird diese durch den Beschleunigungssensor (LSM303) erkannt.
|
|
13
|
+
|
|
14
|
+Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G)
|
|
15
|
+
|
|
16
|
+genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden.
|
|
17
|
+
|
|
18
|
+Für die Filterung der Messwerte werden Medianfilter genutzt. Der Zumo kommuniziert über ein
|
|
19
|
+
|
|
20
|
+Bluetooth-Modul (HC-05) mit anderen Geräten. Die Kommunikation erfolgt seriell ('SERIAL' für USB,
|
|
21
|
+
|
|
22
|
+'SERIAL1' für Bluetooth). Das LCD kann bei Bluetoothnutzung nicht verwendet werden.*/
|
|
23
|
+# 14 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
16
|
24
|
# 15 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino" 2
|
|
25
|
+# 16 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino" 2
|
|
26
|
+# 17 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino" 2
|
17
|
27
|
|
18
|
28
|
Zumo32U4ProximitySensors proxSensors; //Abstandssensoren
|
19
|
29
|
Zumo32U4LineSensors lineSensors; //Liniensensoren
|
|
@@ -22,7 +32,15 @@ Zumo32U4ButtonA buttonA; //Taste A
|
22
|
32
|
Zumo32U4Encoders encoders; //Motorencoder
|
23
|
33
|
LSM303 compass; //Beschleunigungssensor x-Achse
|
24
|
34
|
L3G gyro; //Drehbewegungssensor z-Achse
|
25
|
|
-# 38 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
35
|
+
|
|
36
|
+MedianFilter LineFilter0(3, 0); //erstellen der Filter für Liniensensoren
|
|
37
|
+MedianFilter LineFilter1(3, 0); //Medianfilter geben mittleren Wert einer Reihe aus
|
|
38
|
+MedianFilter LineFilter2(3, 0); //Fenstergröße: 3, Basiswerte: 0 0 0
|
|
39
|
+MedianFilter ProxFilter0(3, 0); //erstellen der Filter für Abstandssensoren
|
|
40
|
+MedianFilter ProxFilter1(3, 0);
|
|
41
|
+MedianFilter ProxFilter2(3, 0);
|
|
42
|
+MedianFilter ProxFilter3(3, 0);
|
|
43
|
+# 48 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
26
|
44
|
int16_t lineValue[3]; //Liniensensoren
|
27
|
45
|
uint16_t lineOffset[3]; //von links (0) nach rechts (2)
|
28
|
46
|
|
|
@@ -71,8 +89,8 @@ void LineSetup()
|
71
|
89
|
uint32_t total[3] = {0, 0, 0};
|
72
|
90
|
for(uint8_t i = 0; i < 120; i++)
|
73
|
91
|
{
|
74
|
|
- if (i > 30 && i <= 90) motors.setSpeeds(106, -100);
|
75
|
|
- else motors.setSpeeds(-106, 100);
|
|
92
|
+ if (i > 30 && i <= 90) motors.setSpeeds(104, -100);
|
|
93
|
+ else motors.setSpeeds(-104, 100);
|
76
|
94
|
lineSensors.read(lineOffset);
|
77
|
95
|
total[0] += lineOffset[0];
|
78
|
96
|
total[1] += lineOffset[1];
|
|
@@ -240,20 +258,20 @@ void LoopTiming()
|
240
|
258
|
void ProxUpdate()
|
241
|
259
|
{
|
242
|
260
|
proxSensors.read();
|
243
|
|
- proxValue[0] = proxSensors.countsLeftWithLeftLeds();
|
244
|
|
- proxValue[1] = proxSensors.countsFrontWithLeftLeds();
|
245
|
|
- proxValue[2] = proxSensors.countsFrontWithRightLeds();
|
246
|
|
- proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
261
|
+ proxValue[0] = ProxFilter0.in(proxSensors.countsLeftWithLeftLeds());
|
|
262
|
+ proxValue[1] = ProxFilter1.in(proxSensors.countsFrontWithLeftLeds());
|
|
263
|
+ proxValue[2] = ProxFilter2.in(proxSensors.countsFrontWithRightLeds());
|
|
264
|
+ proxValue[3] = ProxFilter3.in(proxSensors.countsRightWithRightLeds());
|
247
|
265
|
}
|
248
|
266
|
|
249
|
267
|
//Update Liniensensoren
|
250
|
268
|
void LineUpdate()
|
251
|
269
|
{
|
252
|
270
|
uint16_t lineRaw[3];
|
253
|
|
- lineSensors.read(lineRaw);
|
254
|
|
- lineValue[0] = lineRaw[0] - lineOffset[0];
|
255
|
|
- lineValue[1] = lineRaw[1] - lineOffset[1];
|
256
|
|
- lineValue[2] = lineRaw[2] - lineOffset[2];
|
|
271
|
+ lineSensors.read(lineRaw); //lese Messwerte der Liniensensoren aus
|
|
272
|
+ lineValue[0] = LineFilter0.in(lineRaw[0] - lineOffset[0]); //ziehe Offsetwerte von Messwerten ab und gib diese in Filter ein
|
|
273
|
+ lineValue[1] = LineFilter1.in(lineRaw[1] - lineOffset[1]); //erhält neue mittlere Werte der Filter
|
|
274
|
+ lineValue[2] = LineFilter2.in(lineRaw[2] - lineOffset[2]); //"LineFilter.out" um gefilterte Werte auszugeben ohne neue Werte einzugeben
|
257
|
275
|
}
|
258
|
276
|
|
259
|
277
|
//Update Drehbewegungssensor
|
|
@@ -304,7 +322,7 @@ void Kontaktvermeiden()
|
304
|
322
|
while(proxValue[1] == 0 || proxValue[2] == 0)
|
305
|
323
|
{
|
306
|
324
|
ProxUpdate();
|
307
|
|
- motors.setSpeeds(-53, -50);
|
|
325
|
+ motors.setSpeeds(-52, -50);
|
308
|
326
|
if(lineValue[0] > 150 || lineValue[2] > 120) break;
|
309
|
327
|
}
|
310
|
328
|
motors.setSpeeds(0, 0);
|
|
@@ -324,11 +342,18 @@ void Hindernisumfahren()
|
324
|
342
|
turnAngle = 0;
|
325
|
343
|
rotationAngle = 0;
|
326
|
344
|
GyroUpdate();
|
|
345
|
+ while(rotationAngle < 20)
|
|
346
|
+ {
|
|
347
|
+ GyroUpdate();
|
|
348
|
+ LineUpdate();
|
|
349
|
+ motors.setSpeeds(26, 100);
|
|
350
|
+ }
|
|
351
|
+ GyroUpdate();
|
327
|
352
|
while(rotationAngle < 45)
|
328
|
353
|
{
|
329
|
354
|
GyroUpdate();
|
330
|
355
|
LineUpdate();
|
331
|
|
- motors.setSpeeds(27, 100);
|
|
356
|
+ motors.setSpeeds(26, 100);
|
332
|
357
|
if(lineValue[2] > 120 && lineValue[2] < 500) break;
|
333
|
358
|
}
|
334
|
359
|
//geradeaus über Mittellinie fahren
|
|
@@ -336,7 +361,7 @@ void Hindernisumfahren()
|
336
|
361
|
while(lineValue[2] < 120)
|
337
|
362
|
{
|
338
|
363
|
LineUpdate();
|
339
|
|
- motors.setSpeeds(106, 100);
|
|
364
|
+ motors.setSpeeds(104, 100);
|
340
|
365
|
//if(lineValue[0] > MARKLINE0) break;
|
341
|
366
|
}
|
342
|
367
|
//rechts drehen
|
|
@@ -345,12 +370,12 @@ void Hindernisumfahren()
|
345
|
370
|
{
|
346
|
371
|
GyroUpdate();
|
347
|
372
|
LineUpdate();
|
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);
|
|
373
|
+ if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(104, 0);
|
|
374
|
+ else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(104, 100);
|
|
375
|
+ else motors.setSpeeds(104, 25);
|
351
|
376
|
}
|
352
|
377
|
//geradeaus fahren
|
353
|
|
- motors.setSpeeds(106, 100);
|
|
378
|
+ motors.setSpeeds(104, 100);
|
354
|
379
|
|
355
|
380
|
//Gegenverkehr beachten
|
356
|
381
|
ProxUpdate();
|
|
@@ -359,14 +384,16 @@ void Hindernisumfahren()
|
359
|
384
|
//Schritt 2: Hindernis passieren
|
360
|
385
|
//Serial1.println("Aufholen");
|
361
|
386
|
lastUpdate = millis();
|
362
|
|
- while(proxValue[3] < 6 && eventTime < 3000)
|
|
387
|
+ while(proxValue[3] < 6)
|
363
|
388
|
{
|
364
|
|
- TimeUpdate();
|
365
|
389
|
ProxUpdate();
|
366
|
390
|
LineUpdate();
|
367
|
391
|
Spurhalten();
|
|
392
|
+ TimeUpdate();
|
|
393
|
+ //Serial1.println(eventTime);
|
|
394
|
+ if(eventTime > 3000) break;
|
368
|
395
|
}
|
369
|
|
- //Serial1.println("Überholen");
|
|
396
|
+ //Serial1.println("Vorbeifahren");
|
370
|
397
|
ProxUpdate();
|
371
|
398
|
while(proxValue[3] == 6)
|
372
|
399
|
{
|
|
@@ -379,10 +406,10 @@ void Hindernisumfahren()
|
379
|
406
|
TimeUpdate();
|
380
|
407
|
while(eventTime < 3000)
|
381
|
408
|
{
|
382
|
|
- //Serial1.println(eventTime);
|
383
|
|
- TimeUpdate();
|
384
|
409
|
LineUpdate();
|
385
|
410
|
Spurhalten();
|
|
411
|
+ TimeUpdate();
|
|
412
|
+ //Serial1.println(eventTime);
|
386
|
413
|
}
|
387
|
414
|
//}
|
388
|
415
|
|
|
@@ -391,11 +418,18 @@ void Hindernisumfahren()
|
391
|
418
|
turnAngle = 0;
|
392
|
419
|
rotationAngle = 0;
|
393
|
420
|
GyroUpdate();
|
|
421
|
+ while(rotationAngle > -20)
|
|
422
|
+ {
|
|
423
|
+ GyroUpdate();
|
|
424
|
+ LineUpdate();
|
|
425
|
+ motors.setSpeeds(104, 25);
|
|
426
|
+ }
|
|
427
|
+ GyroUpdate();
|
394
|
428
|
while(rotationAngle > -45)
|
395
|
429
|
{
|
396
|
430
|
GyroUpdate();
|
397
|
431
|
LineUpdate();
|
398
|
|
- motors.setSpeeds(106, 25);
|
|
432
|
+ motors.setSpeeds(104, 25);
|
399
|
433
|
if(lineValue[0] > 150 && lineValue[0] < 500) break;
|
400
|
434
|
}
|
401
|
435
|
//geradeaus über Mittellinie fahren
|
|
@@ -403,7 +437,7 @@ void Hindernisumfahren()
|
403
|
437
|
while(lineValue[0] < 150)
|
404
|
438
|
{
|
405
|
439
|
LineUpdate();
|
406
|
|
- motors.setSpeeds(106, 100);
|
|
440
|
+ motors.setSpeeds(104, 100);
|
407
|
441
|
//if(lineValue[0] > MARKLINE0) break;
|
408
|
442
|
}
|
409
|
443
|
//links drehen
|
|
@@ -413,13 +447,13 @@ void Hindernisumfahren()
|
413
|
447
|
GyroUpdate();
|
414
|
448
|
LineUpdate();
|
415
|
449
|
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
|
|
- else motors.setSpeeds(27, 100);
|
|
450
|
+ else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(104, 100);
|
|
451
|
+ else motors.setSpeeds(26, 100);
|
418
|
452
|
}
|
419
|
453
|
//geradeaus fahren
|
420
|
454
|
//Serial1.println("Umfahren beendet");
|
421
|
455
|
Serial1.print(0);
|
422
|
|
- motors.setSpeeds(106, 100);
|
|
456
|
+ motors.setSpeeds(104, 100);
|
423
|
457
|
}
|
424
|
458
|
|
425
|
459
|
//Funktion Abbiegen
|
|
@@ -433,9 +467,9 @@ void Abbiegen()
|
433
|
467
|
LineUpdate();
|
434
|
468
|
bool leftCode; //links => 1
|
435
|
469
|
bool rightCode; //rechts => 2
|
436
|
|
- if((lineValue[0] > 500 && lineValue[0] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) leftCode = true;
|
|
470
|
+ if((lineValue[0] > 500 && lineValue[0] < 1600) && (lineValue[1] > 300 && lineValue[1] < 800)) leftCode = true;
|
437
|
471
|
else leftCode = false;
|
438
|
|
- if((lineValue[2] > 500 && lineValue[2] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) rightCode = true;
|
|
472
|
+ if((lineValue[2] > 500 && lineValue[2] < 1600) && (lineValue[1] > 300 && lineValue[1] < 800)) rightCode = true;
|
439
|
473
|
else rightCode = false;
|
440
|
474
|
|
441
|
475
|
//Zufallszahl erzeugen
|
|
@@ -458,19 +492,19 @@ void Abbiegen()
|
458
|
492
|
{
|
459
|
493
|
//Serial1.println("links Abbiegen von der Verbindungsstrecke");
|
460
|
494
|
//geradeaus zur Mittellinie fahren
|
461
|
|
- motors.setSpeeds(106, 100 +10);
|
|
495
|
+ motors.setSpeeds(104, 100 +10);
|
462
|
496
|
lastUpdate = millis();
|
463
|
497
|
TimeUpdate();
|
464
|
|
- while(eventTime < 300)
|
|
498
|
+ while(eventTime < 1000)
|
465
|
499
|
{
|
466
|
500
|
TimeUpdate();
|
467
|
|
- motors.setSpeeds(106, 100 +10);
|
|
501
|
+ motors.setSpeeds(104, 100 +10);
|
468
|
502
|
}
|
469
|
503
|
LineUpdate();
|
470
|
504
|
while(lineValue[0] < 150 && lineValue[2] < 120)
|
471
|
505
|
{
|
472
|
506
|
LineUpdate();
|
473
|
|
- motors.setSpeeds(106, 100);
|
|
507
|
+ motors.setSpeeds(104, 100);
|
474
|
508
|
}
|
475
|
509
|
//links drehen
|
476
|
510
|
turnAngle = 0;
|
|
@@ -481,11 +515,11 @@ void Abbiegen()
|
481
|
515
|
GyroUpdate();
|
482
|
516
|
LineUpdate();
|
483
|
517
|
if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(0, 100);
|
484
|
|
- else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(53, 100);
|
485
|
|
- else motors.setSpeeds(27, 100);
|
|
518
|
+ else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(52, 100);
|
|
519
|
+ else motors.setSpeeds(26, 100);
|
486
|
520
|
}
|
487
|
521
|
//geradeaus fahren
|
488
|
|
- motors.setSpeeds(106, 100);
|
|
522
|
+ motors.setSpeeds(104, 100);
|
489
|
523
|
}
|
490
|
524
|
|
491
|
525
|
//links Abbiegen (vom Rundkurs)
|
|
@@ -501,7 +535,7 @@ void Abbiegen()
|
501
|
535
|
{
|
502
|
536
|
GyroUpdate();
|
503
|
537
|
EncoderUpdate();
|
504
|
|
- motors.setSpeeds(27, 100);
|
|
538
|
+ motors.setSpeeds(26, 100);
|
505
|
539
|
//if(driveRotation[1] > 1) break;
|
506
|
540
|
}
|
507
|
541
|
driveRotation[1] = 0;
|
|
@@ -510,11 +544,11 @@ void Abbiegen()
|
510
|
544
|
{
|
511
|
545
|
GyroUpdate();
|
512
|
546
|
EncoderUpdate();
|
513
|
|
- motors.setSpeeds(27, 100);
|
|
547
|
+ motors.setSpeeds(26, 100);
|
514
|
548
|
//if(driveRotation[1] > 1) break;
|
515
|
549
|
}
|
516
|
550
|
//geradeaus fahren
|
517
|
|
- motors.setSpeeds(106, 100);
|
|
551
|
+ motors.setSpeeds(104, 100);
|
518
|
552
|
lastUpdate = millis();
|
519
|
553
|
TimeUpdate();
|
520
|
554
|
while(eventTime < 1000)
|
|
@@ -547,7 +581,7 @@ void Abbiegen()
|
547
|
581
|
GyroUpdate();
|
548
|
582
|
EncoderUpdate();
|
549
|
583
|
LineUpdate();
|
550
|
|
- motors.setSpeeds(106, 25);
|
|
584
|
+ motors.setSpeeds(104, 25);
|
551
|
585
|
if(lineValue[0] > 150 && lineValue[0] < 500) break;
|
552
|
586
|
}
|
553
|
587
|
GyroUpdate();
|
|
@@ -558,10 +592,10 @@ void Abbiegen()
|
558
|
592
|
GyroUpdate();
|
559
|
593
|
LineUpdate();
|
560
|
594
|
if(eventTime > 3000) break;
|
561
|
|
- if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(106, 0);
|
|
595
|
+ if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(104, 0);
|
562
|
596
|
//else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) motors.setSpeeds(-SLOWSPEEDLEFT, -SLOWSPEEDRIGHT);
|
563
|
|
- else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(106, 50);
|
564
|
|
- else motors.setSpeeds(106, 25);
|
|
597
|
+ else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(104, 50);
|
|
598
|
+ else motors.setSpeeds(104, 25);
|
565
|
599
|
}
|
566
|
600
|
}
|
567
|
601
|
|
|
@@ -569,7 +603,7 @@ void Abbiegen()
|
569
|
603
|
else
|
570
|
604
|
{
|
571
|
605
|
//Serial1.println("nicht Abbiegen");
|
572
|
|
- motors.setSpeeds(106, 100);
|
|
606
|
+ motors.setSpeeds(104, 100);
|
573
|
607
|
lastUpdate = millis();
|
574
|
608
|
TimeUpdate();
|
575
|
609
|
while(eventTime < 1000)
|
|
@@ -586,17 +620,20 @@ void Abbiegen()
|
586
|
620
|
//Funktion Spurhalten
|
587
|
621
|
void Spurhalten()
|
588
|
622
|
{
|
|
623
|
+ uint16_t StartTime = millis();
|
|
624
|
+ uint16_t Update = millis();
|
|
625
|
+ uint16_t Time = Update - StartTime;
|
|
626
|
+
|
589
|
627
|
//linke Linie erreicht, nach rechts fahren
|
590
|
628
|
if(lineValue[0] > 150 && lineValue[0] < 500)
|
591
|
629
|
{
|
592
|
630
|
ledYellow(1);
|
593
|
631
|
//Serial1.println("Spur nach rechts korrigieren");
|
594
|
|
- motors.setSpeeds(106/eventSpeed, 25/eventSpeed);
|
595
|
|
- lastUpdate = millis();
|
596
|
|
- TimeUpdate();
|
597
|
|
- while(eventTime < 100)
|
|
632
|
+ motors.setSpeeds(104/eventSpeed, 25/eventSpeed);
|
|
633
|
+ while(Time < 100)
|
598
|
634
|
{
|
599
|
|
- TimeUpdate();
|
|
635
|
+ Update = millis();
|
|
636
|
+ Time = Update - StartTime;
|
600
|
637
|
LineUpdate();
|
601
|
638
|
if(lineValue[2] > 120) break;
|
602
|
639
|
}
|
|
@@ -607,12 +644,11 @@ void Spurhalten()
|
607
|
644
|
{
|
608
|
645
|
ledYellow(1);
|
609
|
646
|
//Serial1.println("Spur nach links korrigieren");
|
610
|
|
- motors.setSpeeds(27/eventSpeed, 100/eventSpeed);
|
611
|
|
- lastUpdate = millis();
|
612
|
|
- TimeUpdate();
|
613
|
|
- while(eventTime < 100)
|
|
647
|
+ motors.setSpeeds(26/eventSpeed, 100/eventSpeed);
|
|
648
|
+ while(Time < 100)
|
614
|
649
|
{
|
615
|
|
- TimeUpdate();
|
|
650
|
+ Update = millis();
|
|
651
|
+ Time = Update - StartTime;
|
616
|
652
|
LineUpdate();
|
617
|
653
|
if(lineValue[0] > 150) break;
|
618
|
654
|
}
|
|
@@ -622,7 +658,7 @@ void Spurhalten()
|
622
|
658
|
else
|
623
|
659
|
{
|
624
|
660
|
ledGreen(1);
|
625
|
|
- motors.setSpeeds(106/eventSpeed, 100/eventSpeed);
|
|
661
|
+ motors.setSpeeds(104/eventSpeed, 100/eventSpeed);
|
626
|
662
|
}
|
627
|
663
|
}
|
628
|
664
|
|
|
@@ -637,7 +673,7 @@ void Spurfinden()
|
637
|
673
|
{
|
638
|
674
|
TimeUpdate();
|
639
|
675
|
LineUpdate();
|
640
|
|
- motors.setSpeeds(-106, -100);
|
|
676
|
+ motors.setSpeeds(-104, -100);
|
641
|
677
|
if(eventTime > 3000) break;
|
642
|
678
|
}
|
643
|
679
|
//Serial1.println("Spur gefunden");
|
|
@@ -695,8 +731,8 @@ void loop()
|
695
|
731
|
eventSpeed = 1.0;
|
696
|
732
|
//if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();
|
697
|
733
|
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();
|
|
734
|
+ else if((lineValue[0] > 500 && lineValue[0] < 1600) && (lineValue[1] > 300 && lineValue[1] < 800)) Abbiegen();
|
|
735
|
+ else if((lineValue[2] > 500 && lineValue[2] < 1600) && (lineValue[1] > 300 && lineValue[1] < 800)) Abbiegen();
|
700
|
736
|
else if(lineValue[1] > 100 && lineValue[1] < 300) Spurfinden();
|
701
|
737
|
else Spurhalten();
|
702
|
738
|
}
|