|
@@ -1,5 +1,5 @@
|
1
|
1
|
//Verfassser: Felix Stange 4056379 MET2016
|
2
|
|
-//Datum: 19.07.2017 erstellt, 18.10.2017 zuletzt geändert
|
|
2
|
+//Datum: 19.07.2017 erstellt, 20.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
|
|
@@ -17,7 +17,7 @@ Zumo32U4ProximitySensors proxSensors; //Abstandssensoren
|
17
|
17
|
Zumo32U4LineSensors lineSensors; //Liniensensoren
|
18
|
18
|
Zumo32U4Motors motors;
|
19
|
19
|
Zumo32U4ButtonA buttonA;
|
20
|
|
-Zumo32U4Encoders encoders;
|
|
20
|
+Zumo32U4Encoders encoders; //Motorencoder
|
21
|
21
|
LSM303 compass; //Beschleunigungssensor x-Achse
|
22
|
22
|
L3G gyro; //Drehbewegungssensor z-Achse
|
23
|
23
|
|
|
@@ -27,10 +27,8 @@ uint16_t lineRaw[3];
|
27
|
27
|
|
28
|
28
|
uint8_t proxValue[4]; //von links (0) nach rechts (3)
|
29
|
29
|
|
30
|
|
-int16_t countsLeft; //Encoder links
|
31
|
|
-int16_t wayLeft;
|
32
|
|
-int16_t countsRight; //Encoder rechts
|
33
|
|
-int16_t wayRight;
|
|
30
|
+int16_t encoderCounts[2]; //von links (0) nach rechts (1)
|
|
31
|
+int16_t driveRotation[2];
|
34
|
32
|
|
35
|
33
|
int32_t rotationAngle = 0; //Drehwinkel
|
36
|
34
|
int32_t turnAngle = 0;
|
|
@@ -150,6 +148,7 @@ void setup()
|
150
|
148
|
Serial1.println("Zumo ready to start, press A");
|
151
|
149
|
ledGreen(1);
|
152
|
150
|
buttonA.waitForButton();
|
|
151
|
+ randomSeed(millis());
|
153
|
152
|
delay(500);
|
154
|
153
|
}
|
155
|
154
|
|
|
@@ -203,10 +202,10 @@ void CompassUpdate()
|
203
|
202
|
//Update Encoder
|
204
|
203
|
void EncoderUpdate()
|
205
|
204
|
{
|
206
|
|
- countsLeft = encoders.getCountsLeft();
|
207
|
|
- wayLeft = countsRight / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
208
|
|
- countsRight = encoders.getCountsRight();
|
209
|
|
- wayRight = countsRight / 910;
|
|
205
|
+ encoderCounts[0] = encoders.getCountsLeft();
|
|
206
|
+ driveRotation[0] = encoderCounts[0] / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
|
207
|
+ encoderCounts[1] = encoders.getCountsRight();
|
|
208
|
+ driveRotation[1] = encoderCounts[1] / 910;
|
210
|
209
|
}
|
211
|
210
|
|
212
|
211
|
/*-----------------------------------------------------------------*/
|
|
@@ -357,9 +356,9 @@ void Crossroad()
|
357
|
356
|
if(randy == 1 && leftCode == true)
|
358
|
357
|
{
|
359
|
358
|
//zur Kreuzungsmitte fahren
|
360
|
|
- wayLeft = 0;
|
361
|
|
- wayRight = 0;
|
362
|
|
- while(wayLeft != 1 && wayRight != 1)
|
|
359
|
+ driveRotation[0] = 0;
|
|
360
|
+ driveRotation[1] = 0;
|
|
361
|
+ while(driveRotation[0] != 1 && driveRotation[1] != 1)
|
363
|
362
|
{
|
364
|
363
|
EncoderUpdate();
|
365
|
364
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
@@ -405,7 +404,7 @@ void SerialOutput()
|
405
|
404
|
PSTR("Abstand: %3d %3d %3d %3d Linie: %6d %6d %6d Richtung: %3c Weg: %6d %6d Winkel: %6d"),
|
406
|
405
|
proxValue[0], proxValue[1], proxValue[2], proxValue[3],
|
407
|
406
|
lineValue[0], lineValue[1], lineValue[2],
|
408
|
|
- dir, wayLeft, wayRight, rotationAngle);
|
|
407
|
+ dir, driveRotation[0], driveRotation[1], rotationAngle);
|
409
|
408
|
Serial1.println(report);
|
410
|
409
|
}
|
411
|
410
|
|
|
@@ -425,10 +424,11 @@ void loop()
|
425
|
424
|
EncoderUpdate();
|
426
|
425
|
|
427
|
426
|
//Funktionsauswahl
|
428
|
|
-/* if(moveRate > 1000) CollisionDetection();
|
|
427
|
+ if(moveRate > 1000) CollisionDetection();
|
|
428
|
+ else if(proxValue[0] == 6 || proxValue[1] == 6) motors.setSpeeds(0, 0);
|
429
|
429
|
else if(proxValue[0] > 4 || proxValue[1] > 4) ObstacleAvoidance();
|
430
|
|
- else if((lineValue[0] > 500 && lineValue[0] < 1500) || (lineValue[2] > 500 && lineValue[2] < 1500) Crossroad();
|
431
|
|
- else TrackKeeper(); */
|
|
430
|
+ else if((lineValue[0] > 500 && lineValue[0] < 1500) || (lineValue[2] > 500 && lineValue[2] < 1500)) Crossroad();
|
|
431
|
+ else TrackKeeper();
|
432
|
432
|
|
433
|
433
|
//Ausgabe über Bluetoothverbindung
|
434
|
434
|
SerialOutput();
|