|
@@ -44,7 +44,9 @@ int16_t moveRate;
|
44
|
44
|
int16_t compassOffset;
|
45
|
45
|
uint16_t compassLastUpdate;
|
46
|
46
|
|
47
|
|
-uint8_t randy; //Zufallszahl für Richtung
|
|
47
|
+uint8_t randy; //Richtungsauswahl Abbiegen geradeaus => 0
|
|
48
|
+bool leftCode; // links => 1
|
|
49
|
+bool rightCode; // rechts => 2
|
48
|
50
|
|
49
|
51
|
int maxSpeed = 200; //Maximale Geschwindigkeit (möglich 400)
|
50
|
52
|
char dir = '0'; //Fahrtrichtung, Ereignis
|
|
@@ -237,6 +239,7 @@ void ObstacleAvoidance()
|
237
|
239
|
LineUpdate();
|
238
|
240
|
motors.setSpeeds(maxSpeed/2, maxSpeed);
|
239
|
241
|
}
|
|
242
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
240
|
243
|
|
241
|
244
|
//Schritt 2: rechts fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
|
242
|
245
|
while(rotationAngle != 0)
|
|
@@ -267,15 +270,15 @@ void ObstacleAvoidance()
|
267
|
270
|
maxSpeed = 200;
|
268
|
271
|
}
|
269
|
272
|
|
270
|
|
- //Schritt 4: rechts bis über Mittellinie fahren
|
271
|
|
- motors.setSpeeds(maxSpeed, maxSpeed/2);
|
272
|
|
- ledYellow(1);
|
|
273
|
+ //Schritt 4: rechts bis über Mittellinie fahren
|
273
|
274
|
rotationAngle = 0;
|
274
|
275
|
GyroUpdate();
|
275
|
|
- while(lineValue[0] < 1000)
|
|
276
|
+ while(lineValue[0] > 1000)
|
276
|
277
|
{
|
277
|
278
|
LineUpdate();
|
|
279
|
+ motors.setSpeeds(maxSpeed, maxSpeed/2);
|
278
|
280
|
}
|
|
281
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
279
|
282
|
|
280
|
283
|
//Schritt 5: links fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
|
281
|
284
|
motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
@@ -295,7 +298,7 @@ void TrackKeeper()
|
295
|
298
|
{
|
296
|
299
|
motors.setSpeeds(maxSpeed, maxSpeed/2);
|
297
|
300
|
ledYellow(1);
|
298
|
|
- delay(200);
|
|
301
|
+ delay(100);
|
299
|
302
|
dir = 'R';
|
300
|
303
|
}
|
301
|
304
|
|
|
@@ -304,19 +307,19 @@ void TrackKeeper()
|
304
|
307
|
{
|
305
|
308
|
motors.setSpeeds(maxSpeed/2, maxSpeed);
|
306
|
309
|
ledYellow(1);
|
307
|
|
- delay(200);
|
|
310
|
+ delay(100);
|
308
|
311
|
dir = 'L';
|
309
|
312
|
}
|
310
|
313
|
|
311
|
314
|
//Linie überfahren, zurücksetzen
|
312
|
315
|
else if(lineValue[1] < 1000)
|
313
|
316
|
{
|
314
|
|
- while(lineValue[0] > 1000)
|
|
317
|
+ ledRed(1);
|
|
318
|
+ while(lineValue[0] > 1000 && lineValue[2] > 1000)
|
315
|
319
|
{
|
316
|
320
|
motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
317
|
321
|
}
|
318
|
|
- ledRed(1);
|
319
|
|
- delay(1000);
|
|
322
|
+ delay(500);
|
320
|
323
|
dir = 'B';
|
321
|
324
|
}
|
322
|
325
|
|
|
@@ -325,7 +328,6 @@ void TrackKeeper()
|
325
|
328
|
{
|
326
|
329
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
327
|
330
|
ledGreen(1);
|
328
|
|
- delay(100);
|
329
|
331
|
dir = 'F';
|
330
|
332
|
}
|
331
|
333
|
}
|
|
@@ -336,14 +338,23 @@ void Crossroad()
|
336
|
338
|
ledYellow(1);
|
337
|
339
|
dir = 'A';
|
338
|
340
|
|
339
|
|
- //Zufallszahl erzeugen
|
340
|
|
- randy = random(3);
|
341
|
|
-
|
342
|
341
|
//Kodierung analysieren
|
|
342
|
+ if(lineValue[0] > 500 && lineValue[0] < 1500) leftCode = true;
|
|
343
|
+ else leftCode = false;
|
|
344
|
+ if(lineValue[2] > 500 && lineValue[2] < 1500) rightCode = true;
|
|
345
|
+ else rightCode = false;
|
343
|
346
|
|
|
347
|
+ //Zufallszahl erzeugen
|
|
348
|
+ if(leftCode == true && rightCode == true) randy = random(1, 3); //1, 2
|
|
349
|
+ else if(leftCode == true && rightCode == false) randy = random(0, 2); //0, 1
|
|
350
|
+ else if(leftCode == false && rightCode == true)
|
|
351
|
+ {
|
|
352
|
+ randy = random(3); //0, (1), 2
|
|
353
|
+ while(randy == 1) randy = random(3); //!1 => 0, 2
|
|
354
|
+ }
|
344
|
355
|
|
345
|
356
|
//links Abbiegen
|
346
|
|
- if(randy == 1)
|
|
357
|
+ if(randy == 1 && leftCode == true)
|
347
|
358
|
{
|
348
|
359
|
//zur Kreuzungsmitte fahren
|
349
|
360
|
wayLeft = 0;
|
|
@@ -360,7 +371,7 @@ void Crossroad()
|
360
|
371
|
while(rotationAngle != 90)
|
361
|
372
|
{
|
362
|
373
|
GyroUpdate();
|
363
|
|
- motors.setSpeeds(0, maxSpeed);
|
|
374
|
+ motors.setSpeeds(0, maxSpeed/2);
|
364
|
375
|
}
|
365
|
376
|
|
366
|
377
|
//geradeaus fahren
|
|
@@ -368,22 +379,23 @@ void Crossroad()
|
368
|
379
|
}
|
369
|
380
|
|
370
|
381
|
//rechts Abbiegen
|
371
|
|
- else if(randy == 2)
|
|
382
|
+ else if(randy == 2 && rightCode == true)
|
372
|
383
|
{
|
373
|
384
|
//rechts drehen
|
374
|
385
|
rotationAngle = 0;
|
375
|
386
|
GyroUpdate();
|
376
|
|
- while(rotationAngle != 90)
|
|
387
|
+ while(rotationAngle != -90)
|
377
|
388
|
{
|
378
|
389
|
GyroUpdate();
|
379
|
|
- motors.setSpeeds(maxSpeed, 0);
|
|
390
|
+ motors.setSpeeds(maxSpeed/2, 0);
|
380
|
391
|
}
|
381
|
392
|
|
382
|
393
|
//geradeaus fahren
|
383
|
394
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
384
|
395
|
}
|
385
|
396
|
|
386
|
|
- else motors.setSpeeds(maxSpeed, maxSpeed);
|
|
397
|
+ //nicht Abbiegen, geradeaus fahren
|
|
398
|
+ else motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
387
|
399
|
}
|
388
|
400
|
|
389
|
401
|
//Funktion Serielle Ausgabe
|
|
@@ -415,7 +427,7 @@ void loop()
|
415
|
427
|
//Funktionsauswahl
|
416
|
428
|
/* if(moveRate > 1000) CollisionDetection();
|
417
|
429
|
else if(proxValue[0] > 4 || proxValue[1] > 4) ObstacleAvoidance();
|
418
|
|
- else if(lineValue[0] < 1000 && lineValue[1] < 1000 && lineValue[2] < 1000) Crossroad();
|
|
430
|
+ else if((lineValue[0] > 500 && lineValue[0] < 1500) || (lineValue[2] > 500 && lineValue[2] < 1500) Crossroad();
|
419
|
431
|
else TrackKeeper(); */
|
420
|
432
|
|
421
|
433
|
//Ausgabe über Bluetoothverbindung
|