|
@@ -1,5 +1,5 @@
|
1
|
1
|
//Verfassser: Felix Stange 4056379 MET2016
|
2
|
|
-//Datum: 19.07.2017 erstellt, 20.10.2017 zuletzt geändert
|
|
2
|
+//Datum: 19.07.2017 erstellt, 25.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
|
|
@@ -230,22 +230,31 @@ void ObstacleAvoidance()
|
230
|
230
|
Serial1.println("obstacle avoidance");
|
231
|
231
|
ledYellow(1);
|
232
|
232
|
|
233
|
|
- //Schritt 1: links bis über Mittellinie fahren
|
|
233
|
+ //Schritt 1: Spurwechsel links
|
|
234
|
+ //links drehen
|
234
|
235
|
rotationAngle = 0;
|
235
|
236
|
GyroUpdate();
|
236
|
|
- while(lineValue[2] > 1000)
|
|
237
|
+ while(rotationAngle < 45)
|
237
|
238
|
{
|
238
|
|
- LineUpdate();
|
|
239
|
+ GyroUpdate();
|
239
|
240
|
motors.setSpeeds(maxSpeed/2, maxSpeed);
|
240
|
241
|
}
|
241
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
242
|
|
-
|
243
|
|
- //Schritt 2: rechts fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
|
244
|
|
- while(rotationAngle != 0)
|
|
242
|
+ //geradeaus über Mittellinie fahren
|
|
243
|
+ LineUpdate();
|
|
244
|
+ while(lineValue[2] > 1000)
|
|
245
|
+ {
|
|
246
|
+ LineUpdate();
|
|
247
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
248
|
+ }
|
|
249
|
+ //rechts drehen
|
|
250
|
+ GyroUpdate();
|
|
251
|
+ while(rotationAngle > 0)
|
245
|
252
|
{
|
246
|
253
|
GyroUpdate();
|
247
|
254
|
motors.setSpeeds(maxSpeed, maxSpeed/2);
|
248
|
255
|
}
|
|
256
|
+ //geradeaus fahren
|
|
257
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
249
|
258
|
|
250
|
259
|
//Gegenverkehr beachten
|
251
|
260
|
proxSensors.read();
|
|
@@ -253,9 +262,9 @@ void ObstacleAvoidance()
|
253
|
262
|
proxValue[2] = proxSensors.countsFrontWithRightLeds();
|
254
|
263
|
if(proxValue[1] < 4 || proxValue[2] < 4)
|
255
|
264
|
{
|
256
|
|
- //Schritt 3: geradeaus fahren bis Hindernis passiert
|
|
265
|
+ //Schritt 2: Hindernis passieren
|
257
|
266
|
maxSpeed = 400;
|
258
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
|
267
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
259
|
268
|
delay(1000);
|
260
|
269
|
proxSensors.read();
|
261
|
270
|
proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
@@ -269,29 +278,36 @@ void ObstacleAvoidance()
|
269
|
278
|
maxSpeed = 200;
|
270
|
279
|
}
|
271
|
280
|
|
272
|
|
- //Schritt 4: rechts bis über Mittellinie fahren
|
|
281
|
+ //Schritt 3: Spurwechsel rechts
|
|
282
|
+ //rechts drehen
|
273
|
283
|
rotationAngle = 0;
|
274
|
284
|
GyroUpdate();
|
275
|
|
- while(lineValue[0] > 1000)
|
|
285
|
+ while(rotationAngle > -45)
|
276
|
286
|
{
|
277
|
|
- LineUpdate();
|
278
|
|
- motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
287
|
+ GyroUpdate();
|
|
288
|
+ motors.setSpeeds(maxSpeed, maxSpeed/2);
|
279
|
289
|
}
|
280
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
281
|
|
-
|
282
|
|
- //Schritt 5: links fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
|
283
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed);
|
284
|
|
- while(rotationAngle != 0)
|
|
290
|
+ //geradeaus über Mittellinie fahren
|
|
291
|
+ LineUpdate();
|
|
292
|
+ while(lineValue[0] > 1000)
|
|
293
|
+ {
|
|
294
|
+ LineUpdate();
|
|
295
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
296
|
+ }
|
|
297
|
+ //links drehen
|
|
298
|
+ GyroUpdate();
|
|
299
|
+ while(rotationAngle < 0)
|
285
|
300
|
{
|
286
|
301
|
GyroUpdate();
|
287
|
|
- TrackKeeper();
|
288
|
|
- }
|
|
302
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
303
|
+ }
|
|
304
|
+ //geradeaus fahren
|
|
305
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
289
|
306
|
}
|
290
|
307
|
|
291
|
308
|
//Funktion Spurhalten
|
292
|
309
|
void TrackKeeper()
|
293
|
310
|
{
|
294
|
|
-
|
295
|
311
|
//linke Linie erreicht, nach rechts fahren
|
296
|
312
|
if(lineValue[0] < 1000)
|
297
|
313
|
{
|
|
@@ -322,7 +338,7 @@ void TrackKeeper()
|
322
|
338
|
dir = 'B';
|
323
|
339
|
}
|
324
|
340
|
|
325
|
|
- //Normale Fahrt
|
|
341
|
+ //normale Fahrt
|
326
|
342
|
else
|
327
|
343
|
{
|
328
|
344
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
@@ -363,7 +379,6 @@ void Crossroad()
|
363
|
379
|
EncoderUpdate();
|
364
|
380
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
365
|
381
|
}
|
366
|
|
-
|
367
|
382
|
//links drehen
|
368
|
383
|
rotationAngle = 0;
|
369
|
384
|
GyroUpdate();
|
|
@@ -372,7 +387,6 @@ void Crossroad()
|
372
|
387
|
GyroUpdate();
|
373
|
388
|
motors.setSpeeds(0, maxSpeed/2);
|
374
|
389
|
}
|
375
|
|
-
|
376
|
390
|
//geradeaus fahren
|
377
|
391
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
378
|
392
|
}
|
|
@@ -388,7 +402,6 @@ void Crossroad()
|
388
|
402
|
GyroUpdate();
|
389
|
403
|
motors.setSpeeds(maxSpeed/2, 0);
|
390
|
404
|
}
|
391
|
|
-
|
392
|
405
|
//geradeaus fahren
|
393
|
406
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
394
|
407
|
}
|