|
@@ -1,5 +1,5 @@
|
1
|
1
|
//Verfassser: Felix Stange 4056379 MET2016
|
2
|
|
-//Datum: 19.07.2017 erstellt, 25.10.2017 zuletzt geändert
|
|
2
|
+//Datum: 19.07.2017 erstellt, 08.12.2017 zuletzt geändert
|
3
|
3
|
//Projekt: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe der
|
4
|
4
|
//Liniensensoren (3), ähnlich einer Spurhalteautomatik (dunkler Belag und helle Streifen).
|
5
|
5
|
//Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es
|
|
@@ -42,10 +42,11 @@ int16_t moveRate;
|
42
|
42
|
int16_t compassOffset;
|
43
|
43
|
uint16_t compassLastUpdate;
|
44
|
44
|
|
|
45
|
+uint16_t LastUpdate;
|
45
|
46
|
int maxSpeed = 200; //Maximale Geschwindigkeit (möglich 400)
|
46
|
|
-char dir = "0"; //Fahrtrichtung, Ereignis
|
47
|
|
-char report[200]; //Ausgabe
|
48
|
|
-char attention; //zeigt Überhol-/Abbiegevorgang an
|
|
47
|
+char dir = 'O'; //Fahrtrichtung, Ereignis
|
|
48
|
+char report[250]; //Ausgabe
|
|
49
|
+int warning = 0; //1 zeigt Überhol-/Abbiegevorgang an
|
49
|
50
|
|
50
|
51
|
/*-----------------------------------------------------------------*/
|
51
|
52
|
|
|
@@ -130,12 +131,13 @@ void setup()
|
130
|
131
|
{
|
131
|
132
|
//Initialisierung der Bluetoothverbindung
|
132
|
133
|
Serial1.begin(9600);
|
133
|
|
- if(Serial1.available()) Serial1.println("Bluetooth verfügbar");
|
|
134
|
+ if(Serial1.available()) Serial1.println("Verbindung hergestellt");
|
134
|
135
|
|
135
|
136
|
//Initialisierung und Kalibrierung der Sensoren
|
136
|
137
|
Serial1.println("Sensorkalibrierung");
|
137
|
138
|
Wire.begin();
|
138
|
139
|
delay(500);
|
|
140
|
+ LastUpdate = micros();
|
139
|
141
|
ProxSetup();
|
140
|
142
|
LineSetup();
|
141
|
143
|
GyroSetup();
|
|
@@ -151,6 +153,14 @@ void setup()
|
151
|
153
|
|
152
|
154
|
/*-----------------------------------------------------------------*/
|
153
|
155
|
|
|
156
|
+//Update Durchlaufzeit
|
|
157
|
+void TimeUpdate()
|
|
158
|
+{
|
|
159
|
+ uint16_t m = micros();
|
|
160
|
+ uint16_t dt = m - LastUpdate;
|
|
161
|
+ LastUpdate = m;
|
|
162
|
+}
|
|
163
|
+
|
154
|
164
|
//Update Abstandssensoren
|
155
|
165
|
void ProxUpdate()
|
156
|
166
|
{
|
|
@@ -210,7 +220,7 @@ void EncoderUpdate()
|
210
|
220
|
//Funktion Kollisionserkennung
|
211
|
221
|
void CollisionDetection()
|
212
|
222
|
{
|
213
|
|
- dir = "X";
|
|
223
|
+ dir = 'X';
|
214
|
224
|
Serial1.println("Aufprall erkannt");
|
215
|
225
|
ledRed(1);
|
216
|
226
|
|
|
@@ -223,8 +233,9 @@ void CollisionDetection()
|
223
|
233
|
//Funktion Hindernisumfahrung
|
224
|
234
|
void ObstacleAvoidance()
|
225
|
235
|
{
|
226
|
|
- dir = "U";
|
227
|
|
- Serial1.println("Überholen");
|
|
236
|
+ dir = 'U';
|
|
237
|
+ Serial1.println("Überholen");
|
|
238
|
+ Serial1.println("1");
|
228
|
239
|
ledYellow(1);
|
229
|
240
|
|
230
|
241
|
//Schritt 1: Spurwechsel links
|
|
@@ -311,7 +322,7 @@ void TrackKeeper()
|
311
|
322
|
motors.setSpeeds(maxSpeed, maxSpeed/2);
|
312
|
323
|
ledYellow(1);
|
313
|
324
|
delay(100);
|
314
|
|
- dir = "R";
|
|
325
|
+ dir = 'R';
|
315
|
326
|
}
|
316
|
327
|
|
317
|
328
|
//rechte Linie erreicht, nach links fahren
|
|
@@ -320,7 +331,7 @@ void TrackKeeper()
|
320
|
331
|
motors.setSpeeds(maxSpeed/2, maxSpeed);
|
321
|
332
|
ledYellow(1);
|
322
|
333
|
delay(100);
|
323
|
|
- dir = "L";
|
|
334
|
+ dir = 'L';
|
324
|
335
|
}
|
325
|
336
|
|
326
|
337
|
//Linie überfahren, zurücksetzen
|
|
@@ -332,7 +343,7 @@ void TrackKeeper()
|
332
|
343
|
motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
333
|
344
|
}
|
334
|
345
|
delay(500);
|
335
|
|
- dir = "B";
|
|
346
|
+ dir = 'B';
|
336
|
347
|
}
|
337
|
348
|
|
338
|
349
|
//normale Fahrt
|
|
@@ -340,7 +351,7 @@ void TrackKeeper()
|
340
|
351
|
{
|
341
|
352
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
342
|
353
|
ledGreen(1);
|
343
|
|
- dir = "F";
|
|
354
|
+ dir = 'F';
|
344
|
355
|
}
|
345
|
356
|
}
|
346
|
357
|
|
|
@@ -348,8 +359,9 @@ void TrackKeeper()
|
348
|
359
|
void Crossroad()
|
349
|
360
|
{
|
350
|
361
|
ledYellow(1);
|
351
|
|
- dir = "A";
|
|
362
|
+ dir = 'A';
|
352
|
363
|
Serial1.println("Abbiegen");
|
|
364
|
+ Serial1.println("1");
|
353
|
365
|
|
354
|
366
|
//Kodierung analysieren
|
355
|
367
|
bool leftCode; //links => 1
|
|
@@ -386,7 +398,9 @@ void Crossroad()
|
386
|
398
|
while(rotationAngle != 90)
|
387
|
399
|
{
|
388
|
400
|
GyroUpdate();
|
389
|
|
- motors.setSpeeds(0, maxSpeed/2);
|
|
401
|
+ if(lineValue[0] < 1000) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
402
|
+ else if(lineValue[2] < 1000) motors.setSpeeds(0, maxSpeed/2);
|
|
403
|
+ else motors.setSpeeds(maxSpeed/4, maxSpeed/2);
|
390
|
404
|
}
|
391
|
405
|
//geradeaus fahren
|
392
|
406
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
@@ -401,7 +415,9 @@ void Crossroad()
|
401
|
415
|
while(rotationAngle != -90)
|
402
|
416
|
{
|
403
|
417
|
GyroUpdate();
|
404
|
|
- motors.setSpeeds(maxSpeed/2, 0);
|
|
418
|
+ if(lineValue[0] < 1000) motors.setSpeeds(maxSpeed/2, 0);
|
|
419
|
+ else if(lineValue[2] < 1000) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
420
|
+ else motors.setSpeeds(maxSpeed/2, maxSpeed/4);
|
405
|
421
|
}
|
406
|
422
|
//geradeaus fahren
|
407
|
423
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
@@ -417,11 +433,11 @@ void Crossroad()
|
417
|
433
|
void SerialOutput()
|
418
|
434
|
{
|
419
|
435
|
snprintf_P(report, sizeof(report),
|
420
|
|
- PSTR("Abstand: %3d %3d %3d %3d Linie: %6d %6d %6d Richtung: %3c Weg: %6d %6d Winkel: %6d"),
|
|
436
|
+ PSTR("Abstand: %3d %3d %3d %3d Linie: %6d %6d %6d Weg: %3d %3d Winkel: %3d Ereignis: %3c Zeit: %3d"),
|
421
|
437
|
proxValue[0], proxValue[1], proxValue[2], proxValue[3],
|
422
|
438
|
lineValue[0], lineValue[1], lineValue[2],
|
423
|
|
- dir, driveRotation[0], driveRotation[1], rotationAngle);
|
424
|
|
- Serial1.println(report);
|
|
439
|
+ driveRotation[0], driveRotation[1], rotationAngle, dir, LastUpdate);
|
|
440
|
+ Serial.println(report);
|
425
|
441
|
}
|
426
|
442
|
|
427
|
443
|
/*-----------------------------------------------------------------*/
|
|
@@ -433,6 +449,7 @@ void loop()
|
433
|
449
|
ledRed(0);
|
434
|
450
|
|
435
|
451
|
//Abfragen der Sensordaten
|
|
452
|
+ TimeUpdate();
|
436
|
453
|
LineUpdate();
|
437
|
454
|
ProxUpdate();
|
438
|
455
|
GyroUpdate();
|
|
@@ -440,11 +457,11 @@ void loop()
|
440
|
457
|
EncoderUpdate();
|
441
|
458
|
|
442
|
459
|
//Funktionsauswahl
|
443
|
|
- attention = Serial1.read();
|
444
|
|
- if(attention == "Überholen" || "Abbiegen")
|
|
460
|
+ warning = Serial1.read();
|
|
461
|
+ if(warning == 1)
|
445
|
462
|
{
|
446
|
463
|
maxSpeed = 100;
|
447
|
|
- if(moveRate > 1000 || proxValue[0] > 4 || proxValue[1] > 4 || lineValue[0] > 500 && lineValue[0] < 1500) ||
|
|
464
|
+ if(moveRate > 1000 || proxValue[0] > 4 || proxValue[1] > 4 || lineValue[0] > 500 && lineValue[0] < 1500 ||
|
448
|
465
|
lineValue[2] > 500 && lineValue[2] < 1500) motors.setSpeeds(0, 0);
|
449
|
466
|
else TrackKeeper();
|
450
|
467
|
}
|