|
@@ -24,13 +24,15 @@ uint16_t lineOffset[3];
|
24
|
24
|
|
25
|
25
|
uint8_t proxValues[4]; //von links (0) nach rechts (3)
|
26
|
26
|
|
27
|
|
-uint32_t turnAngle; //Drehwinkel in °
|
|
27
|
+uint32_t turnAngle; //Drehwinkel
|
28
|
28
|
int16_t turnRate;
|
29
|
29
|
int16_t gyroOffset;
|
30
|
30
|
uint16_t gyroLastUpdate;
|
31
|
31
|
|
32
|
32
|
int16_t compassOffset;
|
33
|
33
|
|
|
34
|
+uint4_t rand; //Zufallszahl für Richtung
|
|
35
|
+
|
34
|
36
|
int maxSpeed = 200; //Maximale Geschwindigkeit (möglich 400)
|
35
|
37
|
char dir; //Fahrtrichtung, Ereignis
|
36
|
38
|
char report[120]; //Ausgabe
|
|
@@ -115,7 +117,7 @@ void setup()
|
115
|
117
|
|
116
|
118
|
//Kalibrierung der Sensoren
|
117
|
119
|
Serial1.println("sensor calibration");
|
118
|
|
- buttonA.waitForButton();
|
|
120
|
+ buttonA.waitForButton();
|
119
|
121
|
LineSensorSetup();
|
120
|
122
|
TurnSensorSetup();
|
121
|
123
|
MoveSensorSetup();
|
|
@@ -201,8 +203,6 @@ void ObstacleAvoidance()
|
201
|
203
|
{
|
202
|
204
|
TurnSensorUpdate();
|
203
|
205
|
}
|
204
|
|
-
|
205
|
|
- ledYellow(0);
|
206
|
206
|
}
|
207
|
207
|
|
208
|
208
|
//Funktion Spurhalten
|
|
@@ -210,7 +210,7 @@ void TrackKeeper()
|
210
|
210
|
{
|
211
|
211
|
|
212
|
212
|
//linke Linie erreicht, nach rechts fahren
|
213
|
|
- if(lineValues[0] < (lineOffset[0] - 200))
|
|
213
|
+ if(lineValues[0] > (lineOffset[0] + 200))
|
214
|
214
|
{
|
215
|
215
|
motors.setSpeeds(maxSpeed, 0);
|
216
|
216
|
ledYellow(1);
|
|
@@ -219,7 +219,7 @@ void TrackKeeper()
|
219
|
219
|
}
|
220
|
220
|
|
221
|
221
|
//rechte Linie erreicht, nach links fahren
|
222
|
|
- else if(lineValues[2] < (lineOffset[2] - 200))
|
|
222
|
+ else if(lineValues[2] > (lineOffset[2] + 200))
|
223
|
223
|
{
|
224
|
224
|
motors.setSpeeds(0, maxSpeed);
|
225
|
225
|
ledYellow(1);
|
|
@@ -228,7 +228,7 @@ void TrackKeeper()
|
228
|
228
|
}
|
229
|
229
|
|
230
|
230
|
//Linie überfahren, zurücksetzen
|
231
|
|
- else if(lineValues[1] < (lineOffset[1] - 100))
|
|
231
|
+ else if(lineValues[1] > (lineOffset[1] + 200))
|
232
|
232
|
{
|
233
|
233
|
motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
234
|
234
|
ledRed(1);
|
|
@@ -261,7 +261,50 @@ void TurnSensorUpdate()
|
261
|
261
|
//Funktion Abbiegen
|
262
|
262
|
void Crossroad()
|
263
|
263
|
{
|
|
264
|
+ ledYellow(1);
|
|
265
|
+ dir = 'A';
|
|
266
|
+
|
|
267
|
+ //Zufallszahl erzeugen
|
|
268
|
+ rand = random(3);
|
|
269
|
+
|
|
270
|
+ //Kodierung analysieren
|
|
271
|
+
|
|
272
|
+
|
|
273
|
+ //links Abbiegen
|
|
274
|
+ if(rand == 1)
|
|
275
|
+ {
|
|
276
|
+ //zur Kreuzungsmitte fahren
|
|
277
|
+ while((lineValues[0] < (lineOffset[0] + 200)) || (lineValues[1] < (lineOffset[1] + 200))
|
|
278
|
+ || (lineValues[2] < (lineOffset[2] + 200))) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
279
|
+
|
|
280
|
+ //links drehen
|
|
281
|
+ turnAngle = 0;
|
|
282
|
+ TurnSensorUpdate();
|
|
283
|
+ while(turnAngle != 90)
|
|
284
|
+ {
|
|
285
|
+ motors.setSpeeds(0, maxSpeed);
|
|
286
|
+ TurnSensorUpdate();
|
|
287
|
+ }
|
|
288
|
+
|
|
289
|
+ //geradeaus fahren
|
|
290
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
291
|
+ }
|
|
292
|
+
|
|
293
|
+ //rechts Abbiegen
|
|
294
|
+ else if(rand == 2)
|
|
295
|
+ {
|
|
296
|
+ //rechts drehen
|
|
297
|
+ turnAngle = 0;
|
|
298
|
+ TurnSensorUpdate();
|
|
299
|
+ while(turnAngle != 90)
|
|
300
|
+ {
|
|
301
|
+ motors.setSpeeds(maxSpeed, 0);
|
|
302
|
+ TurnSensorUpdate();
|
|
303
|
+ }
|
264
|
304
|
|
|
305
|
+ //geradeaus fahren
|
|
306
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
307
|
+ }
|
265
|
308
|
}
|
266
|
309
|
|
267
|
310
|
//Funktion Serielle Ausgabe
|
|
@@ -294,7 +337,8 @@ void loop()
|
294
|
337
|
//Funktionsauswahl
|
295
|
338
|
if((compass.a.x - compassOffset) > 16000) CollisionDetection();
|
296
|
339
|
else if((proxValues[0] || proxValues[1]) > 4) ObstacleAvoidance();
|
297
|
|
- else if (lineValue[0] && lineValue[1] && lineValue[2]) Crossroad();
|
|
340
|
+ else if((lineValues[0] > (lineOffset[0] + 200)) && (lineValues[1] > (lineOffset[1] + 200))
|
|
341
|
+ && (lineValues[2] > (lineOffset[2] + 200))) Crossroad();
|
298
|
342
|
else TrackKeeper();
|
299
|
343
|
|
300
|
344
|
//Ausgabe über Bluetoothverbindung
|