|
|
@@ -1,5 +1,5 @@
|
|
1
|
1
|
//Verfassser: Felix Stange, 4056379, MET2016
|
|
2
|
|
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 15.02.2018
|
|
|
2
|
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 01.03.2018
|
|
3
|
3
|
//Projektbeschreibung: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe
|
|
4
|
4
|
//der Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen).
|
|
5
|
5
|
//Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es
|
|
|
@@ -15,19 +15,19 @@
|
|
15
|
15
|
|
|
16
|
16
|
Zumo32U4ProximitySensors proxSensors; //Abstandssensoren
|
|
17
|
17
|
Zumo32U4LineSensors lineSensors; //Liniensensoren
|
|
18
|
|
-Zumo32U4Motors motors;
|
|
19
|
|
-Zumo32U4ButtonA buttonA;
|
|
|
18
|
+Zumo32U4Motors motors; //Motoren
|
|
|
19
|
+Zumo32U4ButtonA buttonA; //Taste A
|
|
20
|
20
|
Zumo32U4Encoders encoders; //Motorencoder
|
|
21
|
21
|
LSM303 compass; //Beschleunigungssensor x-Achse
|
|
22
|
22
|
L3G gyro; //Drehbewegungssensor z-Achse
|
|
23
|
23
|
|
|
24
|
|
-int16_t lineValue[3]; //von links (0) nach rechts (2)
|
|
25
|
|
-uint16_t lineOffset[3];
|
|
|
24
|
+int16_t lineValue[3]; //Liniensensoren
|
|
|
25
|
+uint16_t lineOffset[3]; //von links (0) nach rechts (2)
|
|
26
|
26
|
|
|
27
|
|
-uint8_t proxValue[4]; //von links (0) nach rechts (3)
|
|
|
27
|
+uint8_t proxValue[4]; //Abstandssensoren v.l.(0)n.r.(3)
|
|
28
|
28
|
|
|
29
|
|
-int16_t encoderCounts[2]; //von links (0) nach rechts (1)
|
|
30
|
|
-int16_t driveRotation[2];
|
|
|
29
|
+int16_t encoderCounts[2]; //Anzahl der Umdrehungen
|
|
|
30
|
+int16_t driveRotation[2]; //von links (0) nach rechts (1)
|
|
31
|
31
|
|
|
32
|
32
|
int32_t rotationAngle = 0; //Drehwinkel
|
|
33
|
33
|
int32_t turnAngle = 0;
|
|
|
@@ -43,10 +43,10 @@ uint16_t compassLastUpdate;
|
|
43
|
43
|
|
|
44
|
44
|
uint16_t LastUpdate = 0; //Systemzeit
|
|
45
|
45
|
uint16_t CycleTime = 0; //Zykluszeit
|
|
46
|
|
-int maxSpeed = 200; //Maximale Geschwindigkeit (möglich 400)
|
|
|
46
|
+int maxSpeed = 200; //Maximale Geschwindigkeit (max. 400)
|
|
47
|
47
|
char dir; //Fahrtrichtung, Ereignis
|
|
48
|
48
|
char report[200]; //Ausgabe
|
|
49
|
|
-String btData;
|
|
|
49
|
+String btData; //Gelesene Daten aus Bluetooth
|
|
50
|
50
|
|
|
51
|
51
|
/*-----------------------------------------------------------------*/
|
|
52
|
52
|
|
|
|
@@ -95,7 +95,7 @@ void GyroSetup()
|
|
95
|
95
|
int32_t total = 0;
|
|
96
|
96
|
for (uint16_t i = 0; i < 1024; i++)
|
|
97
|
97
|
{
|
|
98
|
|
- while(!gyro.readReg(L3G::STATUS_REG) & 0x08);
|
|
|
98
|
+ while(!gyro.readReg(L3G::STATUS_REG) & 0x08); //Fehlerabfrage
|
|
99
|
99
|
gyro.read();
|
|
100
|
100
|
total += gyro.g.z;
|
|
101
|
101
|
}
|
|
|
@@ -218,20 +218,23 @@ void EncoderUpdate()
|
|
218
|
218
|
|
|
219
|
219
|
/*-----------------------------------------------------------------*/
|
|
220
|
220
|
|
|
221
|
|
- //Funktion Kollisionserkennung
|
|
222
|
|
-void Kollisionserkennung()
|
|
|
221
|
+//Funktion Kontaktvermeiden
|
|
|
222
|
+void Kontaktvermeiden()
|
|
223
|
223
|
{
|
|
224
|
224
|
dir = 'X';
|
|
225
|
|
- Serial1.println("Kollision");
|
|
|
225
|
+ Serial1.println("Kontaktvermeiden");
|
|
226
|
226
|
ledRed(1);
|
|
227
|
227
|
|
|
228
|
228
|
motors.setSpeeds(0, 0);
|
|
229
|
229
|
delay(500);
|
|
230
|
|
- while(lineValue[0] < 300 && lineValue[2] < 300)
|
|
|
230
|
+ while(proxValue[1] == 0 || proxValue[2] == 0)
|
|
231
|
231
|
{
|
|
|
232
|
+ ProxUpdate();
|
|
232
|
233
|
motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
234
|
+ if(lineValue[0] > 300 || lineValue[2] > 300) return;
|
|
233
|
235
|
}
|
|
234
|
236
|
delay(500);
|
|
|
237
|
+ Serial1.println("Vermeiden beendet");
|
|
235
|
238
|
}
|
|
236
|
239
|
|
|
237
|
240
|
//Funktion Hindernisumfahrung
|
|
|
@@ -268,21 +271,17 @@ void Hindernisumfahren()
|
|
268
|
271
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
269
|
272
|
|
|
270
|
273
|
//Gegenverkehr beachten
|
|
271
|
|
- proxSensors.read();
|
|
272
|
|
- proxValue[1] = proxSensors.countsFrontWithLeftLeds();
|
|
273
|
|
- proxValue[2] = proxSensors.countsFrontWithRightLeds();
|
|
|
274
|
+ ProxUpdate();
|
|
274
|
275
|
if(proxValue[1] < 5 || proxValue[2] < 5)
|
|
275
|
276
|
{
|
|
276
|
277
|
//Schritt 2: Hindernis passieren
|
|
277
|
278
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
278
|
279
|
delay(1000);
|
|
279
|
|
- proxSensors.read();
|
|
280
|
|
- proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
|
280
|
+ ProxUpdate();
|
|
281
|
281
|
while(proxValue[3] > 4)
|
|
282
|
282
|
{
|
|
283
|
283
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
284
|
|
- proxSensors.read();
|
|
285
|
|
- proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
|
284
|
+ ProxUpdate();
|
|
286
|
285
|
Spurhalten();
|
|
287
|
286
|
}
|
|
288
|
287
|
}
|
|
|
@@ -323,7 +322,7 @@ void Abbiegen()
|
|
323
|
322
|
ledYellow(1);
|
|
324
|
323
|
Serial1.println("Abbiegen");
|
|
325
|
324
|
|
|
326
|
|
- //Kodierung analysieren
|
|
|
325
|
+ //Markierung analysieren
|
|
327
|
326
|
bool leftCode; //links => 1
|
|
328
|
327
|
bool rightCode; //rechts => 2
|
|
329
|
328
|
if(lineValue[0] > 1000) leftCode = true;
|
|
|
@@ -420,13 +419,14 @@ void Spurhalten()
|
|
420
|
419
|
{
|
|
421
|
420
|
dir = 'B';
|
|
422
|
421
|
ledRed(1);
|
|
423
|
|
- Serial1.println("Spur verlassen");
|
|
|
422
|
+ Serial1.println("Spurfinden");
|
|
424
|
423
|
Serial1.println("1");
|
|
425
|
424
|
while(lineValue[0] < 300 && lineValue[2] < 300) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
|
|
426
|
425
|
{
|
|
427
|
426
|
motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
428
|
427
|
}
|
|
429
|
428
|
delay(500);
|
|
|
429
|
+ Serial1.println("Spur gefunden");
|
|
430
|
430
|
}
|
|
431
|
431
|
|
|
432
|
432
|
//normale Fahrt
|
|
|
@@ -469,7 +469,8 @@ void loop()
|
|
469
|
469
|
//Funktionsauswahl
|
|
470
|
470
|
btData = Serial1.readString();
|
|
471
|
471
|
//verfügbare Funktionen bei laufenden Manövern
|
|
472
|
|
- if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kollision")
|
|
|
472
|
+ if(btData == "Abbiegen" || btData == "Hindernisumfahren"
|
|
|
473
|
+ || btData == "Kontaktvermeiden"|| btData == "Spurfinden")
|
|
473
|
474
|
{
|
|
474
|
475
|
maxSpeed = 100;
|
|
475
|
476
|
if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || lineValue[0] > 1000 ||
|
|
|
@@ -479,7 +480,7 @@ void loop()
|
|
479
|
480
|
//verfügbare Funktionen im Normalfall
|
|
480
|
481
|
else
|
|
481
|
482
|
{
|
|
482
|
|
- if(moveRate > 1000) Kollisionserkennung();
|
|
|
483
|
+ if(moveRate > 1000) Kontaktvermeiden();
|
|
483
|
484
|
else if(proxValue[1] == 6 || proxValue[2] == 6) motors.setSpeeds(0, 0);
|
|
484
|
485
|
else if(proxValue[1] == 5 || proxValue[2] == 5) Hindernisumfahren();
|
|
485
|
486
|
else if(lineValue[0] > 1000 || lineValue[2] > 1000) Abbiegen();
|
|
|
@@ -488,4 +489,4 @@ void loop()
|
|
488
|
489
|
|
|
489
|
490
|
//Ausgabe über Bluetoothverbindung
|
|
490
|
491
|
SerialOutput();
|
|
491
|
|
-}
|
|
|
492
|
+}
|
fstange