|
@@ -1,5 +1,5 @@
|
1
|
1
|
//Verfassser: Felix Stange 4056379 MET2016
|
2
|
|
-//Datum: 19.07.2017 erstellt, 11.10.2017 zuletzt geändert
|
|
2
|
+//Datum: 19.07.2017 erstellt, 18.10.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
|
|
@@ -21,15 +21,15 @@ Zumo32U4Encoders encoders;
|
21
|
21
|
LSM303 compass; //Beschleunigungssensor x-Achse
|
22
|
22
|
L3G gyro; //Drehbewegungssensor z-Achse
|
23
|
23
|
|
24
|
|
-uint16_t lineValue[3]; //von links (0) nach rechts (2)
|
|
24
|
+uint16_t lineValue[3]; //von links (0) nach rechts (2)
|
25
|
25
|
uint16_t lineOffset[3];
|
26
|
26
|
uint16_t lineRaw[3];
|
27
|
27
|
|
28
|
|
-uint8_t proxValue[4]; //von links (0) nach rechts (3)
|
|
28
|
+uint8_t proxValue[4]; //von links (0) nach rechts (3)
|
29
|
29
|
|
30
|
|
-int16_t countsLeft; //Encoder rechts
|
|
30
|
+int16_t countsLeft; //Encoder links
|
31
|
31
|
int16_t wayLeft;
|
32
|
|
-int16_t countsRight; //Encoder links
|
|
32
|
+int16_t countsRight; //Encoder rechts
|
33
|
33
|
int16_t wayRight;
|
34
|
34
|
|
35
|
35
|
int32_t rotationAngle = 0; //Drehwinkel
|
|
@@ -146,6 +146,7 @@ void setup()
|
146
|
146
|
|
147
|
147
|
//Zumo bereit zu starten
|
148
|
148
|
Serial1.println("Zumo ready to start, press A");
|
|
149
|
+ ledGreen(1);
|
149
|
150
|
buttonA.waitForButton();
|
150
|
151
|
delay(500);
|
151
|
152
|
}
|
|
@@ -198,11 +199,10 @@ void CompassUpdate()
|
198
|
199
|
}
|
199
|
200
|
|
200
|
201
|
//Update Encoder
|
201
|
|
-
|
202
|
202
|
void EncoderUpdate()
|
203
|
203
|
{
|
204
|
204
|
countsLeft = encoders.getCountsLeft();
|
205
|
|
- wayLeft = countsRight / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
|
205
|
+ wayLeft = countsRight / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
206
|
206
|
countsRight = encoders.getCountsRight();
|
207
|
207
|
wayRight = countsRight / 910;
|
208
|
208
|
}
|
|
@@ -229,27 +229,27 @@ void ObstacleAvoidance()
|
229
|
229
|
Serial1.println("obstacle avoidance");
|
230
|
230
|
ledYellow(1);
|
231
|
231
|
|
232
|
|
- //Schritt 1: links bis über Mittellinie fahren
|
233
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
232
|
+ //Schritt 1: links bis über Mittellinie fahren
|
234
|
233
|
rotationAngle = 0;
|
235
|
234
|
GyroUpdate();
|
236
|
235
|
while(lineValue[2] > 1000)
|
237
|
236
|
{
|
238
|
237
|
LineUpdate();
|
|
238
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed);
|
239
|
239
|
}
|
240
|
240
|
|
241
|
|
- //Schritt 2: rechts fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
|
242
|
|
- motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
241
|
+ //Schritt 2: rechts fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
|
243
|
242
|
while(rotationAngle != 0)
|
244
|
243
|
{
|
245
|
244
|
GyroUpdate();
|
|
245
|
+ motors.setSpeeds(maxSpeed, maxSpeed/2);
|
246
|
246
|
}
|
247
|
247
|
|
248
|
248
|
//Gegenverkehr beachten
|
249
|
249
|
proxSensors.read();
|
250
|
250
|
proxValue[1] = proxSensors.countsFrontWithLeftLeds();
|
251
|
251
|
proxValue[2] = proxSensors.countsFrontWithRightLeds();
|
252
|
|
- if((proxValue[1] || proxValue[2]) < 4)
|
|
252
|
+ if(proxValue[1] < 4 || proxValue[2] < 4)
|
253
|
253
|
{
|
254
|
254
|
//Schritt 3: geradeaus fahren bis Hindernis passiert
|
255
|
255
|
maxSpeed = 400;
|
|
@@ -259,6 +259,7 @@ void ObstacleAvoidance()
|
259
|
259
|
proxValue[3] = proxSensors.countsRightWithRightLeds();
|
260
|
260
|
while(proxValue[3] > 4)
|
261
|
261
|
{
|
|
262
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
262
|
263
|
proxSensors.read();
|
263
|
264
|
proxValue[3] = proxSensors.countsRightWithRightLeds();
|
264
|
265
|
TrackKeeper();
|
|
@@ -267,11 +268,11 @@ void ObstacleAvoidance()
|
267
|
268
|
}
|
268
|
269
|
|
269
|
270
|
//Schritt 4: rechts bis über Mittellinie fahren
|
270
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
271
|
+ motors.setSpeeds(maxSpeed, maxSpeed/2);
|
271
|
272
|
ledYellow(1);
|
272
|
273
|
rotationAngle = 0;
|
273
|
274
|
GyroUpdate();
|
274
|
|
- while(lineValue[0] - 200)
|
|
275
|
+ while(lineValue[0] < 1000)
|
275
|
276
|
{
|
276
|
277
|
LineUpdate();
|
277
|
278
|
}
|
|
@@ -281,6 +282,7 @@ void ObstacleAvoidance()
|
281
|
282
|
while(rotationAngle != 0)
|
282
|
283
|
{
|
283
|
284
|
GyroUpdate();
|
|
285
|
+ TrackKeeper();
|
284
|
286
|
}
|
285
|
287
|
}
|
286
|
288
|
|
|
@@ -291,7 +293,7 @@ void TrackKeeper()
|
291
|
293
|
//linke Linie erreicht, nach rechts fahren
|
292
|
294
|
if(lineValue[0] < 1000)
|
293
|
295
|
{
|
294
|
|
- motors.setSpeeds(maxSpeed, 0);
|
|
296
|
+ motors.setSpeeds(maxSpeed, maxSpeed/2);
|
295
|
297
|
ledYellow(1);
|
296
|
298
|
delay(200);
|
297
|
299
|
dir = 'R';
|
|
@@ -300,7 +302,7 @@ void TrackKeeper()
|
300
|
302
|
//rechte Linie erreicht, nach links fahren
|
301
|
303
|
else if(lineValue[2] < 1000)
|
302
|
304
|
{
|
303
|
|
- motors.setSpeeds(0, maxSpeed);
|
|
305
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed);
|
304
|
306
|
ledYellow(1);
|
305
|
307
|
delay(200);
|
306
|
308
|
dir = 'L';
|
|
@@ -310,7 +312,9 @@ void TrackKeeper()
|
310
|
312
|
else if(lineValue[1] < 1000)
|
311
|
313
|
{
|
312
|
314
|
while(lineValue[0] > 1000)
|
313
|
|
- motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
315
|
+ {
|
|
316
|
+ motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
317
|
+ }
|
314
|
318
|
ledRed(1);
|
315
|
319
|
delay(1000);
|
316
|
320
|
dir = 'B';
|
|
@@ -342,7 +346,9 @@ void Crossroad()
|
342
|
346
|
if(randy == 1)
|
343
|
347
|
{
|
344
|
348
|
//zur Kreuzungsmitte fahren
|
345
|
|
- while((wayLeft != 1) && (wayRight != 1))
|
|
349
|
+ wayLeft = 0;
|
|
350
|
+ wayRight = 0;
|
|
351
|
+ while(wayLeft != 1 && wayRight != 1)
|
346
|
352
|
{
|
347
|
353
|
EncoderUpdate();
|
348
|
354
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
@@ -353,8 +359,8 @@ void Crossroad()
|
353
|
359
|
GyroUpdate();
|
354
|
360
|
while(rotationAngle != 90)
|
355
|
361
|
{
|
356
|
|
- motors.setSpeeds(0, maxSpeed);
|
357
|
|
- GyroUpdate();
|
|
362
|
+ GyroUpdate();
|
|
363
|
+ motors.setSpeeds(0, maxSpeed);
|
358
|
364
|
}
|
359
|
365
|
|
360
|
366
|
//geradeaus fahren
|
|
@@ -369,13 +375,15 @@ void Crossroad()
|
369
|
375
|
GyroUpdate();
|
370
|
376
|
while(rotationAngle != 90)
|
371
|
377
|
{
|
372
|
|
- motors.setSpeeds(maxSpeed, 0);
|
373
|
|
- GyroUpdate();
|
|
378
|
+ GyroUpdate();
|
|
379
|
+ motors.setSpeeds(maxSpeed, 0);
|
374
|
380
|
}
|
375
|
381
|
|
376
|
382
|
//geradeaus fahren
|
377
|
383
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
378
|
384
|
}
|
|
385
|
+
|
|
386
|
+ else motors.setSpeeds(maxSpeed, maxSpeed);
|
379
|
387
|
}
|
380
|
388
|
|
381
|
389
|
//Funktion Serielle Ausgabe
|
|
@@ -405,9 +413,9 @@ void loop()
|
405
|
413
|
EncoderUpdate();
|
406
|
414
|
|
407
|
415
|
//Funktionsauswahl
|
408
|
|
-/* if((compass.a.x - compassOffset) > 1000) CollisionDetection();
|
409
|
|
- else if((proxValue[0] || proxValue[1]) > 4) ObstacleAvoidance();
|
410
|
|
- else if((lineValue[0] < 1000)) && (lineValue[1] < 1000)) && (lineValue[2] < 1000))) Crossroad();
|
|
416
|
+/* if(moveRate > 1000) CollisionDetection();
|
|
417
|
+ else if(proxValue[0] > 4 || proxValue[1] > 4) ObstacleAvoidance();
|
|
418
|
+ else if(lineValue[0] < 1000 && lineValue[1] < 1000 && lineValue[2] < 1000) Crossroad();
|
411
|
419
|
else TrackKeeper(); */
|
412
|
420
|
|
413
|
421
|
//Ausgabe über Bluetoothverbindung
|