|
@@ -2,7 +2,7 @@
|
2
|
2
|
#line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
3
|
3
|
#line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
4
|
4
|
//Verfassser: Felix Stange, 4056379, MET2016
|
5
|
|
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 14.05.2018
|
|
5
|
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 17.05.2018
|
6
|
6
|
//Projektthema: Untersuchungen und Implementierung eines automatisierten Fahrens mittels Pololu Zumo
|
7
|
7
|
/*Kurzbeschreibung: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe
|
8
|
8
|
der Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen).
|
|
@@ -26,11 +26,13 @@ Zumo32U4Encoders encoders; //Motorencoder
|
26
|
26
|
LSM303 compass; //Beschleunigungssensor x-Achse
|
27
|
27
|
L3G gyro; //Drehbewegungssensor z-Achse
|
28
|
28
|
|
|
29
|
+//Medianfilter geben mittleren Wert einer Reihe mit ungerader Anzahl aus
|
29
|
30
|
MedianFilter LineFilter0(3, 0); //erstellen der Filter für Liniensensoren
|
30
|
|
-MedianFilter LineFilter1(3, 0); //Medianfilter geben mittleren Wert einer Reihe aus
|
31
|
|
-MedianFilter LineFilter2(3, 0); //Fenstergröße: 3, Basiswerte: 0 0 0
|
|
31
|
+MedianFilter LineFilter1(3, 0); //Fenstergröße: 3, Basiswerte: 0 0 0
|
|
32
|
+MedianFilter LineFilter2(3, 0);
|
|
33
|
+
|
32
|
34
|
MedianFilter ProxFilter0(5, 0); //erstellen der Filter für Abstandssensoren
|
33
|
|
-MedianFilter ProxFilter1(5, 0);
|
|
35
|
+MedianFilter ProxFilter1(5, 0); //Fenstergröße: 5, Basiswerte: 0 0 0 0 0
|
34
|
36
|
MedianFilter ProxFilter2(5, 0);
|
35
|
37
|
MedianFilter ProxFilter3(5, 0);
|
36
|
38
|
|
|
@@ -40,6 +42,7 @@ MedianFilter ProxFilter3(5, 0);
|
40
|
42
|
#define SIGN0 500
|
41
|
43
|
#define SIGN1 300
|
42
|
44
|
#define SIGN2 500
|
|
45
|
+
|
43
|
46
|
#define MAXSPEED 400
|
44
|
47
|
#define FULLSPEEDLEFT 104
|
45
|
48
|
#define HALFSPEEDLEFT 52
|
|
@@ -61,64 +64,66 @@ int32_t turnAngle = 0;
|
61
|
64
|
int16_t turnRate;
|
62
|
65
|
int16_t gyroOffset;
|
63
|
66
|
uint16_t gyroLastUpdate;
|
64
|
|
-
|
65
|
|
-//int32_t moveDisplay = 0; //Beschleunigung
|
66
|
|
-//int32_t moveDistance = 0;
|
67
|
|
-int16_t moveRate;
|
68
|
|
-int16_t compassOffset;
|
69
|
|
-//uint16_t compassLastUpdate;
|
|
67
|
+
|
|
68
|
+int16_t compassRate; //Beschleunigung
|
|
69
|
+int16_t compassOffset;
|
|
70
|
+int16_t compassLastUpdate;
|
70
|
71
|
|
71
|
72
|
uint16_t lastUpdate = 0; //Systemzeit
|
72
|
|
-uint16_t eventTime = 0; //Zeit seit letzter Geschwindigkeitsänderung
|
73
|
|
-char report[200]; //Ausgabe
|
|
73
|
+uint16_t eventTime = 0; //Zeitdifferenz
|
|
74
|
+uint16_t stopUpdate = 0; //Systemzeit
|
|
75
|
+uint16_t stopTime = 0; //Zeit seit letztem Manöver
|
74
|
76
|
float eventSpeed = 1; //vermindert die Geschwindigkeit bei Manövern
|
75
|
77
|
int btData = 0; //Gelesene Daten aus Bluetooth
|
76
|
|
-bool alarm = false; //zeigt Manöver abhängig von btData
|
|
78
|
+bool btBuffer = false; //puffert Daten von btData
|
|
79
|
+bool stop = false; //Sperrt Funktion Kontaktvermeiden
|
77
|
80
|
|
78
|
81
|
/*-----------------------------------------------------------------*/
|
79
|
82
|
|
80
|
83
|
//Setup Bluetoothverbindung
|
81
|
|
-#line 78 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
84
|
+#line 81 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
82
|
85
|
void BlueSetup();
|
83
|
|
-#line 88 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
86
|
+#line 91 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
84
|
87
|
void LineSetup();
|
85
|
|
-#line 117 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
88
|
+#line 120 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
86
|
89
|
void ProxSetup();
|
87
|
|
-#line 123 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
90
|
+#line 126 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
88
|
91
|
void GyroSetup();
|
89
|
|
-#line 156 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
92
|
+#line 159 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
90
|
93
|
void CompassSetup();
|
91
|
|
-#line 187 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
94
|
+#line 190 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
92
|
95
|
void setup();
|
93
|
|
-#line 213 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
96
|
+#line 217 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
94
|
97
|
void TimeUpdate();
|
95
|
|
-#line 219 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
98
|
+#line 224 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
99
|
+void StopUpdate();
|
|
100
|
+#line 230 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
96
|
101
|
void LoopTiming();
|
97
|
|
-#line 263 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
102
|
+#line 274 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
103
|
+void SerielleAusgabe();
|
|
104
|
+#line 289 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
98
|
105
|
void ProxUpdate();
|
99
|
|
-#line 294 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
106
|
+#line 316 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
100
|
107
|
void LineUpdate();
|
101
|
|
-#line 304 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
108
|
+#line 326 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
102
|
109
|
void GyroUpdate();
|
103
|
|
-#line 317 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
110
|
+#line 339 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
104
|
111
|
void CompassUpdate();
|
105
|
|
-#line 330 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
112
|
+#line 348 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
106
|
113
|
void EncoderUpdate();
|
107
|
|
-#line 341 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
114
|
+#line 359 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
108
|
115
|
void Kontaktvermeiden();
|
109
|
|
-#line 360 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
116
|
+#line 393 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
110
|
117
|
void Hindernisumfahren();
|
111
|
|
-#line 486 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
118
|
+#line 525 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
112
|
119
|
void Abbiegen();
|
113
|
|
-#line 647 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
120
|
+#line 686 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
114
|
121
|
void Spurhalten();
|
115
|
|
-#line 692 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
122
|
+#line 736 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
116
|
123
|
void Spurfinden();
|
117
|
|
-#line 710 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
118
|
|
-void SerielleAusgabe();
|
119
|
|
-#line 725 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
124
|
+#line 757 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
120
|
125
|
void loop();
|
121
|
|
-#line 78 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
126
|
+#line 81 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
122
|
127
|
void BlueSetup()
|
123
|
128
|
{
|
124
|
129
|
Serial1.begin(9600); //Initialisierung mit Datengeschwindigkeit(Baud)
|
|
@@ -217,7 +222,7 @@ void CompassSetup()
|
217
|
222
|
int32_t total = 0;
|
218
|
223
|
for (uint16_t i = 0; i < 1024; i++)
|
219
|
224
|
{
|
220
|
|
- compass.read();
|
|
225
|
+ compass.readAcc();
|
221
|
226
|
total += compass.a.x;
|
222
|
227
|
}
|
223
|
228
|
compassOffset = total / 1024;
|
|
@@ -248,18 +253,26 @@ void setup()
|
248
|
253
|
buttonA.waitForButton();
|
249
|
254
|
randomSeed(millis());
|
250
|
255
|
delay(500);
|
|
256
|
+ stopUpdate = millis();
|
251
|
257
|
//Serial1.println("Start");
|
252
|
258
|
}
|
253
|
259
|
|
254
|
260
|
/*-----------------------------------------------------------------*/
|
255
|
261
|
|
256
|
|
-//Update Durchlaufzeit
|
|
262
|
+//Update Zeitdifferenz für alle Funktionen
|
257
|
263
|
void TimeUpdate()
|
258
|
264
|
{
|
259
|
265
|
uint16_t m = millis();
|
260
|
266
|
eventTime = m - lastUpdate;
|
261
|
267
|
}
|
262
|
268
|
|
|
269
|
+//Update Zeit für Kontaktvermeiden
|
|
270
|
+void StopUpdate()
|
|
271
|
+{
|
|
272
|
+ uint16_t m = millis();
|
|
273
|
+ stopTime = m - stopUpdate;
|
|
274
|
+}
|
|
275
|
+
|
263
|
276
|
void LoopTiming()
|
264
|
277
|
{
|
265
|
278
|
const int AL = 100; // Arraylänge, NUR GERADE Zahlen verwenden!
|
|
@@ -303,6 +316,21 @@ void LoopTiming()
|
303
|
316
|
}
|
304
|
317
|
}
|
305
|
318
|
|
|
319
|
+//Funktion Serielle Ausgabe
|
|
320
|
+void SerielleAusgabe()
|
|
321
|
+{
|
|
322
|
+ char report[200];
|
|
323
|
+ snprintf_P(report, sizeof(report),
|
|
324
|
+ PSTR("Abstand: %d %d %d %d Linie: %d %d %d"),
|
|
325
|
+ proxValue[0], proxValue[1], proxValue[2], proxValue[3],
|
|
326
|
+ lineValue[0], lineValue[1], lineValue[2]);
|
|
327
|
+ Serial1.println(report);
|
|
328
|
+ snprintf_P(report, sizeof(report),
|
|
329
|
+ PSTR("Weg: %d %d Winkel: %d Beschleunigung: %d"),
|
|
330
|
+ driveRotation[0], driveRotation[1], rotationAngle, compassRate);
|
|
331
|
+ Serial1.println(report);
|
|
332
|
+}
|
|
333
|
+
|
306
|
334
|
//Update Abstandssensoren
|
307
|
335
|
void ProxUpdate()
|
308
|
336
|
{
|
|
@@ -320,10 +348,6 @@ void ProxUpdate()
|
320
|
348
|
proxValue[1] = ProxFilter1.in(proxSensors.countsFrontWithLeftLeds());
|
321
|
349
|
proxValue[2] = ProxFilter2.in(proxSensors.countsFrontWithRightLeds());
|
322
|
350
|
proxValue[3] = ProxFilter3.in(proxSensors.countsRightWithRightLeds());
|
323
|
|
- // Serial.println(proxValue[0]);
|
324
|
|
- // Serial.println(proxValue[1]);
|
325
|
|
- // Serial.println(proxValue[2]);
|
326
|
|
- // Serial.println(proxValue[3]);
|
327
|
351
|
done = true;
|
328
|
352
|
}
|
329
|
353
|
else
|
|
@@ -360,22 +384,18 @@ void GyroUpdate()
|
360
|
384
|
//Update Beschleunigungssensor
|
361
|
385
|
void CompassUpdate()
|
362
|
386
|
{
|
363
|
|
- compass.read(); //Rohwert 16384 entspricht 1g (9,81m/s²) bzw.
|
364
|
|
- moveRate = compass.a.x - compassOffset; //bei +/-2g Messbereich 0,61mg/LSB
|
365
|
|
- // uint16_t m = micros();
|
366
|
|
- // uint16_t dt = m - compassLastUpdate;
|
367
|
|
- // compassLastUpdate = m;
|
368
|
|
- // int32_t d = (int32_t)moveRate * dt;
|
369
|
|
- // moveDistance += (int64_t)d * dt >> 14;
|
370
|
|
- // moveDisplay = moveDistance * 9,81 / 1000;
|
|
387
|
+ compass.read(); //Rohwert 16384 entspricht 1g (9,81m/s²) bzw. bei +/-2g Messbereich 0,61mg/LSB
|
|
388
|
+ int16_t x = compass.a.x - compassOffset;
|
|
389
|
+ compassRate = x - compassLastUpdate;
|
|
390
|
+ compassLastUpdate = x;
|
371
|
391
|
}
|
372
|
392
|
|
373
|
393
|
//Update Encoder
|
374
|
394
|
void EncoderUpdate()
|
375
|
395
|
{
|
376
|
|
- encoderCounts[0] = encoders.getCountsLeft();
|
|
396
|
+ encoderCounts[0] += encoders.getCountsAndResetLeft();
|
377
|
397
|
driveRotation[0] = encoderCounts[0] / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
378
|
|
- encoderCounts[1] = encoders.getCountsRight();
|
|
398
|
+ encoderCounts[1] += encoders.getCountsAndResetRight();
|
379
|
399
|
driveRotation[1] = encoderCounts[1] / 910;
|
380
|
400
|
}
|
381
|
401
|
|
|
@@ -392,10 +412,25 @@ void Kontaktvermeiden()
|
392
|
412
|
while(proxValue[1] == 0 || proxValue[2] == 0)
|
393
|
413
|
{
|
394
|
414
|
ProxUpdate();
|
|
415
|
+ LineUpdate();
|
395
|
416
|
motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
|
396
|
|
- if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) break;
|
|
417
|
+ if(lineValue[0] > MARKLINE0 || lineValue[1] > MARKLINE1 || lineValue[2] > MARKLINE2) break;
|
|
418
|
+ }
|
|
419
|
+ lastUpdate = millis();
|
|
420
|
+ TimeUpdate();
|
|
421
|
+ while(eventTime < 1000)
|
|
422
|
+ {
|
|
423
|
+ TimeUpdate();
|
|
424
|
+ LineUpdate();
|
|
425
|
+ motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
|
|
426
|
+ if(lineValue[0] > MARKLINE0 || lineValue[1] > MARKLINE1 || lineValue[2] > MARKLINE2) break;
|
397
|
427
|
}
|
398
|
428
|
motors.setSpeeds(0, 0);
|
|
429
|
+ compassLastUpdate = 0;
|
|
430
|
+ compassRate = 0;
|
|
431
|
+ CompassUpdate();
|
|
432
|
+ stop = false;
|
|
433
|
+ stopUpdate = millis();
|
399
|
434
|
//Serial1.println("Vermeiden beendet");
|
400
|
435
|
Serial1.print(0);
|
401
|
436
|
}
|
|
@@ -426,14 +461,15 @@ void Hindernisumfahren()
|
426
|
461
|
motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
427
|
462
|
if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
|
428
|
463
|
}
|
|
464
|
+
|
429
|
465
|
//geradeaus über Mittellinie fahren
|
430
|
466
|
LineUpdate();
|
431
|
467
|
while(lineValue[2] < MARKLINE2)
|
432
|
468
|
{
|
433
|
469
|
LineUpdate();
|
434
|
470
|
motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
435
|
|
- //if(lineValue[0] > MARKLINE0) break;
|
436
|
471
|
}
|
|
472
|
+
|
437
|
473
|
//rechts drehen
|
438
|
474
|
GyroUpdate();
|
439
|
475
|
while(rotationAngle > 10)
|
|
@@ -444,12 +480,13 @@ void Hindernisumfahren()
|
444
|
480
|
else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
445
|
481
|
else motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
|
446
|
482
|
}
|
|
483
|
+
|
447
|
484
|
//geradeaus fahren
|
448
|
485
|
motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
449
|
486
|
|
450
|
487
|
//Gegenverkehr beachten
|
451
|
488
|
ProxUpdate();
|
452
|
|
- //if(proxValue[1] < 5 || proxValue[2] < 5)
|
|
489
|
+ //if(proxValue[1] == 6 && proxValue[2] == 6)
|
453
|
490
|
//{
|
454
|
491
|
//Schritt 2: Hindernis passieren
|
455
|
492
|
//Serial1.println("Aufholen");
|
|
@@ -502,14 +539,15 @@ void Hindernisumfahren()
|
502
|
539
|
motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
|
503
|
540
|
if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
|
504
|
541
|
}
|
|
542
|
+
|
505
|
543
|
//geradeaus über Mittellinie fahren
|
506
|
544
|
LineUpdate();
|
507
|
545
|
while(lineValue[0] < MARKLINE0)
|
508
|
546
|
{
|
509
|
547
|
LineUpdate();
|
510
|
548
|
motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
511
|
|
- //if(lineValue[0] > MARKLINE0) break;
|
512
|
549
|
}
|
|
550
|
+
|
513
|
551
|
//links drehen
|
514
|
552
|
GyroUpdate();
|
515
|
553
|
while(rotationAngle < -10)
|
|
@@ -520,10 +558,13 @@ void Hindernisumfahren()
|
520
|
558
|
else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
521
|
559
|
else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
522
|
560
|
}
|
|
561
|
+
|
523
|
562
|
//geradeaus fahren
|
524
|
563
|
//Serial1.println("Umfahren beendet");
|
525
|
|
- Serial1.print(0);
|
526
|
564
|
motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
565
|
+ stop = false;
|
|
566
|
+ stopUpdate = millis();
|
|
567
|
+ Serial1.print(0);
|
527
|
568
|
}
|
528
|
569
|
|
529
|
570
|
//Funktion Abbiegen
|
|
@@ -574,8 +615,9 @@ void Abbiegen()
|
574
|
615
|
while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
|
575
|
616
|
{
|
576
|
617
|
LineUpdate();
|
577
|
|
- motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
618
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10);
|
578
|
619
|
}
|
|
620
|
+
|
579
|
621
|
//links drehen
|
580
|
622
|
turnAngle = 0;
|
581
|
623
|
rotationAngle = 0;
|
|
@@ -588,6 +630,7 @@ void Abbiegen()
|
588
|
630
|
else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(HALFSPEEDLEFT, FULLSPEEDRIGHT);
|
589
|
631
|
else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
590
|
632
|
}
|
|
633
|
+
|
591
|
634
|
//geradeaus fahren
|
592
|
635
|
motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
593
|
636
|
}
|
|
@@ -597,26 +640,22 @@ void Abbiegen()
|
597
|
640
|
{
|
598
|
641
|
//Serial1.println("links Abbiegen vom Rundkurs");
|
599
|
642
|
//links drehen
|
|
643
|
+ motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
600
|
644
|
turnAngle = 0;
|
601
|
645
|
rotationAngle = 0;
|
602
|
|
- driveRotation[1] = 0;
|
603
|
646
|
GyroUpdate();
|
604
|
647
|
while(rotationAngle < 40)
|
605
|
648
|
{
|
606
|
649
|
GyroUpdate();
|
607
|
|
- EncoderUpdate();
|
608
|
650
|
motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
609
|
|
- //if(driveRotation[1] > 1) break;
|
610
|
651
|
}
|
611
|
|
- driveRotation[1] = 0;
|
612
|
652
|
GyroUpdate();
|
613
|
653
|
while(rotationAngle < 85)
|
614
|
654
|
{
|
615
|
655
|
GyroUpdate();
|
616
|
|
- EncoderUpdate();
|
617
|
656
|
motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
618
|
|
- //if(driveRotation[1] > 1) break;
|
619
|
657
|
}
|
|
658
|
+
|
620
|
659
|
//geradeaus fahren
|
621
|
660
|
motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
622
|
661
|
lastUpdate = millis();
|
|
@@ -625,6 +664,7 @@ void Abbiegen()
|
625
|
664
|
{
|
626
|
665
|
TimeUpdate();
|
627
|
666
|
LineUpdate();
|
|
667
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
628
|
668
|
if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
|
629
|
669
|
}
|
630
|
670
|
lastUpdate = millis();
|
|
@@ -642,14 +682,13 @@ void Abbiegen()
|
642
|
682
|
{
|
643
|
683
|
//Serial1.println("rechts Abbiegen");
|
644
|
684
|
//rechts drehen
|
|
685
|
+ motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
|
645
|
686
|
turnAngle = 0;
|
646
|
687
|
rotationAngle = 0;
|
647
|
|
- driveRotation[0] = 0;
|
648
|
688
|
GyroUpdate();
|
649
|
689
|
while(rotationAngle > -40)
|
650
|
690
|
{
|
651
|
691
|
GyroUpdate();
|
652
|
|
- EncoderUpdate();
|
653
|
692
|
LineUpdate();
|
654
|
693
|
motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
|
655
|
694
|
if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
|
|
@@ -683,6 +722,8 @@ void Abbiegen()
|
683
|
722
|
Spurhalten();
|
684
|
723
|
}
|
685
|
724
|
}
|
|
725
|
+ stop = false;
|
|
726
|
+ stopUpdate = millis();
|
686
|
727
|
//Serial1.println("Abbiegen beendet");
|
687
|
728
|
Serial1.print(0);
|
688
|
729
|
}
|
|
@@ -699,7 +740,7 @@ void Spurhalten()
|
699
|
740
|
{
|
700
|
741
|
ledYellow(1);
|
701
|
742
|
//Serial1.println("Spur nach rechts korrigieren");
|
702
|
|
- motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
|
|
743
|
+ motors.setSpeeds((int)FULLSPEEDLEFT/eventSpeed, (int)SLOWSPEEDRIGHT/eventSpeed);
|
703
|
744
|
while(Time < 100)
|
704
|
745
|
{
|
705
|
746
|
Update = millis();
|
|
@@ -707,6 +748,8 @@ void Spurhalten()
|
707
|
748
|
LineUpdate();
|
708
|
749
|
if(lineValue[2] > MARKLINE2) break;
|
709
|
750
|
}
|
|
751
|
+ stop = false;
|
|
752
|
+ stopUpdate = millis();
|
710
|
753
|
}
|
711
|
754
|
|
712
|
755
|
//rechte Linie erreicht, nach links fahren
|
|
@@ -714,7 +757,7 @@ void Spurhalten()
|
714
|
757
|
{
|
715
|
758
|
ledYellow(1);
|
716
|
759
|
//Serial1.println("Spur nach links korrigieren");
|
717
|
|
- motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);
|
|
760
|
+ motors.setSpeeds((int)SLOWSPEEDLEFT/eventSpeed, (int)FULLSPEEDRIGHT/eventSpeed);
|
718
|
761
|
while(Time < 100)
|
719
|
762
|
{
|
720
|
763
|
Update = millis();
|
|
@@ -722,13 +765,16 @@ void Spurhalten()
|
722
|
765
|
LineUpdate();
|
723
|
766
|
if(lineValue[0] > MARKLINE0) break;
|
724
|
767
|
}
|
|
768
|
+ stop = false;
|
|
769
|
+ stopUpdate = millis();
|
725
|
770
|
}
|
726
|
771
|
|
727
|
772
|
//normale Fahrt
|
728
|
773
|
else
|
729
|
774
|
{
|
730
|
775
|
ledGreen(1);
|
731
|
|
- motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);
|
|
776
|
+ motors.setSpeeds((int)FULLSPEEDLEFT/eventSpeed, (int)FULLSPEEDRIGHT/eventSpeed);
|
|
777
|
+ stop = true;
|
732
|
778
|
}
|
733
|
779
|
}
|
734
|
780
|
|
|
@@ -746,24 +792,12 @@ void Spurfinden()
|
746
|
792
|
motors.setSpeeds(-FULLSPEEDLEFT, -FULLSPEEDRIGHT);
|
747
|
793
|
if(eventTime > 3000) break;
|
748
|
794
|
}
|
|
795
|
+ stop = false;
|
|
796
|
+ stopUpdate = millis();
|
749
|
797
|
//Serial1.println("Spur gefunden");
|
750
|
798
|
Serial1.print(0);
|
751
|
799
|
}
|
752
|
800
|
|
753
|
|
-//Funktion Serielle Ausgabe
|
754
|
|
-void SerielleAusgabe()
|
755
|
|
-{
|
756
|
|
- snprintf_P(report, sizeof(report),
|
757
|
|
- PSTR("Abstand: %d %d %d %d Linie: %d %d %d"),
|
758
|
|
- proxValue[0], proxValue[1], proxValue[2], proxValue[3],
|
759
|
|
- lineValue[0], lineValue[1], lineValue[2]);
|
760
|
|
- Serial1.println(report);
|
761
|
|
- snprintf_P(report, sizeof(report),
|
762
|
|
- PSTR("Weg: %d %d Winkel: %d Beschleunigung: %d"),
|
763
|
|
- driveRotation[0], driveRotation[1], rotationAngle, moveRate);
|
764
|
|
- Serial1.println(report);
|
765
|
|
-}
|
766
|
|
-
|
767
|
801
|
/*-----------------------------------------------------------------*/
|
768
|
802
|
|
769
|
803
|
void loop()
|
|
@@ -780,27 +814,31 @@ void loop()
|
780
|
814
|
CompassUpdate();
|
781
|
815
|
LineUpdate();
|
782
|
816
|
TimeUpdate();
|
|
817
|
+ StopUpdate();
|
783
|
818
|
|
784
|
819
|
//Funktionsauswahl
|
785
|
|
- //btData = Serial1.readString();
|
786
|
820
|
if(Serial1.available() > 0) btData = Serial1.read();
|
787
|
|
- if(btData == '1') alarm = true;
|
788
|
|
- else if(btData == '0') alarm = false;
|
|
821
|
+ if(btData == '1') btBuffer = true;
|
|
822
|
+ else if(btData == '0') btBuffer = false;
|
|
823
|
+
|
789
|
824
|
//verfügbare Funktionen bei laufenden Manövern
|
790
|
|
- //if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kontaktvermeiden"|| btData == "Spurfinden")
|
791
|
|
- if(alarm == true)
|
|
825
|
+ if(btBuffer == true)
|
792
|
826
|
{
|
793
|
827
|
//Serial1.println("Verstanden");
|
794
|
828
|
eventSpeed = 1.4;
|
795
|
|
- if(moveRate > 1500 || (proxValue[1] == 6 && proxValue[2] == 6) || lineValue[0] > SIGN0 || lineValue[2] > SIGN2 || lineValue[1] > MARKLINE1) motors.setSpeeds(0, 0);
|
|
829
|
+ if(proxValue[0] == 6 || (proxValue[1] == 6 && proxValue[2] == 6)) motors.setSpeeds(0, 0);
|
|
830
|
+ else if(stop == true && stopTime > 2000 && abs(compassRate) > 3500) motors.setSpeeds(0, 0);
|
|
831
|
+ else if(lineValue[0] > SIGN0 || lineValue[2] > SIGN2 || lineValue[1] > MARKLINE1) motors.setSpeeds(0, 0);
|
796
|
832
|
else Spurhalten();
|
797
|
833
|
}
|
|
834
|
+
|
798
|
835
|
//verfügbare Funktionen im Normalfall
|
799
|
836
|
else
|
800
|
837
|
{
|
801
|
838
|
eventSpeed = 1.0;
|
802
|
|
- //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();
|
803
|
|
- if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
|
|
839
|
+ if(stop == true && stopTime > 2000 && abs(compassRate) > 3500) Kontaktvermeiden();
|
|
840
|
+ else if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
|
|
841
|
+ else if(lineValue[0] > 1600 && lineValue[2] > 1600) motors.setSpeeds(0, 0);
|
804
|
842
|
else if((lineValue[0] > SIGN0 && lineValue[0] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) Abbiegen();
|
805
|
843
|
else if((lineValue[2] > SIGN2 && lineValue[2] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) Abbiegen();
|
806
|
844
|
else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) Spurfinden();
|