|
@@ -1,7 +1,7 @@
|
|
|
//Verfassser: Felix Stange 4056379 MET2016
|
|
|
-//Datum: 19.07.2017 erstellt, 08.12.2017 zuletzt geändert
|
|
|
+//Datum: 19.07.2017 erstellt, 02.02.2018 zuletzt geändert
|
|
|
//Projekt: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe der
|
|
|
-//Liniensensoren (3), ähnlich einer Spurhalteautomatik (dunkler Belag und helle Streifen).
|
|
|
+//Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen).
|
|
|
//Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es
|
|
|
//dennoch zu einer Kollision, wird diese durch die Beschleunigungssensoren (LSM303) erkannt.
|
|
|
//Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G)
|
|
@@ -21,9 +21,8 @@ Zumo32U4Encoders encoders; //Motorencoder
|
|
|
LSM303 compass; //Beschleunigungssensor x-Achse
|
|
|
L3G gyro; //Drehbewegungssensor z-Achse
|
|
|
|
|
|
-uint16_t lineValue[3]; //von links (0) nach rechts (2)
|
|
|
+int16_t lineValue[3]; //von links (0) nach rechts (2)
|
|
|
uint16_t lineOffset[3];
|
|
|
-uint16_t lineRaw[3];
|
|
|
|
|
|
uint8_t proxValue[4]; //von links (0) nach rechts (3)
|
|
|
|
|
@@ -42,10 +41,11 @@ int16_t moveRate;
|
|
|
int16_t compassOffset;
|
|
|
uint16_t compassLastUpdate;
|
|
|
|
|
|
-uint16_t LastUpdate;
|
|
|
+uint16_t LastUpdate = 0; //Systemzeit
|
|
|
+uint16_t CycleTime = 0; //Zykluszeit
|
|
|
int maxSpeed = 200; //Maximale Geschwindigkeit (möglich 400)
|
|
|
-char dir = 'O'; //Fahrtrichtung, Ereignis
|
|
|
-char report[250]; //Ausgabe
|
|
|
+char dir; //Fahrtrichtung, Ereignis
|
|
|
+char report[200]; //Ausgabe
|
|
|
int warning = 0; //1 zeigt Überhol-/Abbiegevorgang an
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
@@ -144,7 +144,7 @@ void setup()
|
|
|
CompassSetup();
|
|
|
|
|
|
//Zumo bereit zu starten
|
|
|
- Serial1.println("Zumo bereit, drücke A");
|
|
|
+ Serial1.println("Zumo bereit, starte mit A");
|
|
|
ledGreen(1);
|
|
|
buttonA.waitForButton();
|
|
|
randomSeed(millis());
|
|
@@ -157,7 +157,7 @@ void setup()
|
|
|
void TimeUpdate()
|
|
|
{
|
|
|
uint16_t m = micros();
|
|
|
- uint16_t dt = m - LastUpdate;
|
|
|
+ CycleTime = m - LastUpdate;
|
|
|
LastUpdate = m;
|
|
|
}
|
|
|
|
|
@@ -174,6 +174,7 @@ void ProxUpdate()
|
|
|
//Updaten Liniensensoren
|
|
|
void LineUpdate()
|
|
|
{
|
|
|
+ uint16_t lineRaw[3];
|
|
|
lineSensors.read(lineRaw);
|
|
|
lineValue[0] = lineRaw[0] - lineOffset[0];
|
|
|
lineValue[1] = lineRaw[1] - lineOffset[1];
|
|
@@ -210,7 +211,7 @@ void CompassUpdate()
|
|
|
void EncoderUpdate()
|
|
|
{
|
|
|
encoderCounts[0] = encoders.getCountsLeft();
|
|
|
- driveRotation[0] = encoderCounts[0] / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
|
|
+ driveRotation[0] = encoderCounts[0] / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
|
|
encoderCounts[1] = encoders.getCountsRight();
|
|
|
driveRotation[1] = encoderCounts[1] / 910;
|
|
|
}
|
|
@@ -218,23 +219,26 @@ void EncoderUpdate()
|
|
|
/*-----------------------------------------------------------------*/
|
|
|
|
|
|
//Funktion Kollisionserkennung
|
|
|
-void CollisionDetection()
|
|
|
+void Kollisionserkennung()
|
|
|
{
|
|
|
dir = 'X';
|
|
|
- Serial1.println("Aufprall erkannt");
|
|
|
+ Serial1.println("Kollision erkannt");
|
|
|
+ Serial1.println("1");
|
|
|
ledRed(1);
|
|
|
-
|
|
|
motors.setSpeeds(0, 0);
|
|
|
delay(500);
|
|
|
- motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
+ while(lineValue[0] < 300 && lineValue[2] < 300)
|
|
|
+ {
|
|
|
+ motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
+ }
|
|
|
delay(500);
|
|
|
}
|
|
|
|
|
|
//Funktion Hindernisumfahrung
|
|
|
-void ObstacleAvoidance()
|
|
|
+void Hindernisumfahren()
|
|
|
{
|
|
|
dir = 'U';
|
|
|
- Serial1.println("Überholen");
|
|
|
+ Serial1.println("Hindernis umfahren");
|
|
|
Serial1.println("1");
|
|
|
ledYellow(1);
|
|
|
|
|
@@ -249,7 +253,7 @@ void ObstacleAvoidance()
|
|
|
}
|
|
|
//geradeaus über Mittellinie fahren
|
|
|
LineUpdate();
|
|
|
- while(lineValue[2] > 1000)
|
|
|
+ while(lineValue[2] < 300)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
@@ -280,7 +284,7 @@ void ObstacleAvoidance()
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
proxSensors.read();
|
|
|
proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
|
- TrackKeeper();
|
|
|
+ Spurhalten();
|
|
|
}
|
|
|
}
|
|
|
|
|
@@ -295,7 +299,7 @@ void ObstacleAvoidance()
|
|
|
}
|
|
|
//geradeaus über Mittellinie fahren
|
|
|
LineUpdate();
|
|
|
- while(lineValue[0] > 1000)
|
|
|
+ while(lineValue[0] < 300)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
@@ -310,65 +314,23 @@ void ObstacleAvoidance()
|
|
|
//geradeaus fahren
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
|
|
|
- Serial1.println("Überholen beendet");
|
|
|
-}
|
|
|
-
|
|
|
-//Funktion Spurhalten
|
|
|
-void TrackKeeper()
|
|
|
-{
|
|
|
- //linke Linie erreicht, nach rechts fahren
|
|
|
- if(lineValue[0] < 1000)
|
|
|
- {
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
- ledYellow(1);
|
|
|
- delay(100);
|
|
|
- dir = 'R';
|
|
|
- }
|
|
|
-
|
|
|
- //rechte Linie erreicht, nach links fahren
|
|
|
- else if(lineValue[2] < 1000)
|
|
|
- {
|
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
- ledYellow(1);
|
|
|
- delay(100);
|
|
|
- dir = 'L';
|
|
|
- }
|
|
|
-
|
|
|
- //Linie überfahren, zurücksetzen
|
|
|
- else if(lineValue[1] < 1000)
|
|
|
- {
|
|
|
- ledRed(1);
|
|
|
- while(lineValue[0] > 1000 && lineValue[2] > 1000)
|
|
|
- {
|
|
|
- motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
- }
|
|
|
- delay(500);
|
|
|
- dir = 'B';
|
|
|
- }
|
|
|
-
|
|
|
- //normale Fahrt
|
|
|
- else
|
|
|
- {
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
- ledGreen(1);
|
|
|
- dir = 'F';
|
|
|
- }
|
|
|
+ Serial1.println("Umfahren beendet");
|
|
|
}
|
|
|
|
|
|
//Funktion Abbiegen
|
|
|
-void Crossroad()
|
|
|
+void Abbiegen()
|
|
|
{
|
|
|
- ledYellow(1);
|
|
|
dir = 'A';
|
|
|
+ ledYellow(1);
|
|
|
Serial1.println("Abbiegen");
|
|
|
Serial1.println("1");
|
|
|
|
|
|
//Kodierung analysieren
|
|
|
bool leftCode; //links => 1
|
|
|
bool rightCode; //rechts => 2
|
|
|
- if(lineValue[0] > 500 && lineValue[0] < 1500) leftCode = true;
|
|
|
+ if(lineValue[0] > 1000) leftCode = true;
|
|
|
else leftCode = false;
|
|
|
- if(lineValue[2] > 500 && lineValue[2] < 1500) rightCode = true;
|
|
|
+ if(lineValue[2] > 1000) rightCode = true;
|
|
|
else rightCode = false;
|
|
|
|
|
|
//Zufallszahl erzeugen
|
|
@@ -387,7 +349,7 @@ void Crossroad()
|
|
|
//zur Kreuzungsmitte fahren
|
|
|
driveRotation[0] = 0;
|
|
|
driveRotation[1] = 0;
|
|
|
- while(driveRotation[0] != 1 && driveRotation[1] != 1)
|
|
|
+ while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < 300 && lineValue[2] < 300)
|
|
|
{
|
|
|
EncoderUpdate();
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
@@ -398,16 +360,17 @@ void Crossroad()
|
|
|
while(rotationAngle != 90)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- if(lineValue[0] < 1000) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
- else if(lineValue[2] < 1000) motors.setSpeeds(0, maxSpeed/2);
|
|
|
- else motors.setSpeeds(maxSpeed/4, maxSpeed/2);
|
|
|
+ if(lineValue[0] > 300 && lineValue[0] < 500) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
+ else if(lineValue[1] > 300 && lineValue[1] < 500) motors.setSpeeds(-maxSpeed/4, -maxSpeed/4);
|
|
|
+ else if(lineValue[2] > 300 && lineValue[2] < 500) motors.setSpeeds(0, maxSpeed/2);
|
|
|
+ else motors.setSpeeds(maxSpeed/4, maxSpeed/2);
|
|
|
}
|
|
|
//geradeaus fahren
|
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
}
|
|
|
|
|
|
//rechts Abbiegen
|
|
|
- else if(randy == 2 && rightCode == true)
|
|
|
+ else if(randy == 2 && rightCode == true)
|
|
|
{
|
|
|
//rechts drehen
|
|
|
rotationAngle = 0;
|
|
@@ -415,9 +378,10 @@ void Crossroad()
|
|
|
while(rotationAngle != -90)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- if(lineValue[0] < 1000) motors.setSpeeds(maxSpeed/2, 0);
|
|
|
- else if(lineValue[2] < 1000) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
- else motors.setSpeeds(maxSpeed/2, maxSpeed/4);
|
|
|
+ if(lineValue[0] > 300 && lineValue[0] < 500) motors.setSpeeds(maxSpeed/2, 0);
|
|
|
+ else if(lineValue[1] > 300 && lineValue[1] < 500) motors.setSpeeds(-maxSpeed/4, -maxSpeed/4);
|
|
|
+ else if(lineValue[2] > 300 && lineValue[2] < 500) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
+ else motors.setSpeeds(maxSpeed/2, maxSpeed/4);
|
|
|
}
|
|
|
//geradeaus fahren
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
@@ -425,19 +389,67 @@ void Crossroad()
|
|
|
|
|
|
//nicht Abbiegen, geradeaus fahren
|
|
|
else motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
+ delay(500);
|
|
|
|
|
|
Serial1.println("Abbiegen beendet");
|
|
|
}
|
|
|
|
|
|
+//Funktion Spurhalten
|
|
|
+void Spurhalten()
|
|
|
+{
|
|
|
+ //linke Linie erreicht, nach rechts fahren
|
|
|
+ if(lineValue[0] > 300 && lineValue[0] < 500)
|
|
|
+ {
|
|
|
+ dir = 'R';
|
|
|
+ ledYellow(1);
|
|
|
+ Serial1.println("Spur nach rechts korrigieren");
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
+ delay(100);
|
|
|
+ }
|
|
|
+
|
|
|
+ //rechte Linie erreicht, nach links fahren
|
|
|
+ else if(lineValue[2] > 300 && lineValue[2] < 500)
|
|
|
+ {
|
|
|
+ dir = 'L';
|
|
|
+ ledYellow(1);
|
|
|
+ Serial1.println("Spur nach links korrigieren");
|
|
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
+ delay(100);
|
|
|
+ }
|
|
|
+
|
|
|
+ //Linie überfahren, zurücksetzen
|
|
|
+ else if(lineValue[1] > 300 && lineValue[1] < 500)
|
|
|
+ {
|
|
|
+ dir = 'B';
|
|
|
+ ledRed(1);
|
|
|
+ Serial1.println("Spur verlassen");
|
|
|
+ Serial1.println("1");
|
|
|
+ while(lineValue[0] < 300 && lineValue[2] < 300) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
|
|
|
+ {
|
|
|
+ motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
+ }
|
|
|
+ delay(500);
|
|
|
+ }
|
|
|
+
|
|
|
+ //normale Fahrt
|
|
|
+ else
|
|
|
+ {
|
|
|
+ dir = 'F';
|
|
|
+ ledGreen(1);
|
|
|
+ Serial1.println("Spur folgen");
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
//Funktion Serielle Ausgabe
|
|
|
void SerialOutput()
|
|
|
{
|
|
|
snprintf_P(report, sizeof(report),
|
|
|
- PSTR("Abstand: %3d %3d %3d %3d Linie: %6d %6d %6d Weg: %3d %3d Winkel: %3d Ereignis: %3c Zeit: %3d"),
|
|
|
+ PSTR("Abstand: %d %d %d %d Linie: %d %d %d Weg: %d %d Winkel: %d Zeit: %d"),
|
|
|
proxValue[0], proxValue[1], proxValue[2], proxValue[3],
|
|
|
lineValue[0], lineValue[1], lineValue[2],
|
|
|
- driveRotation[0], driveRotation[1], rotationAngle, dir, LastUpdate);
|
|
|
- Serial.println(report);
|
|
|
+ driveRotation[0], driveRotation[1], rotationAngle, CycleTime);
|
|
|
+ Serial1.println(report);
|
|
|
}
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
@@ -458,20 +470,20 @@ void loop()
|
|
|
|
|
|
//Funktionsauswahl
|
|
|
warning = Serial1.read();
|
|
|
- if(warning == 1)
|
|
|
+ if(warning == 1) //verfügbare Funktionen bei laufenden Manövern
|
|
|
{
|
|
|
maxSpeed = 100;
|
|
|
- if(moveRate > 1000 || proxValue[0] > 4 || proxValue[1] > 4 || lineValue[0] > 500 && lineValue[0] < 1500 ||
|
|
|
- lineValue[2] > 500 && lineValue[2] < 1500) motors.setSpeeds(0, 0);
|
|
|
- else TrackKeeper();
|
|
|
+ if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || lineValue[0] > 1000 ||
|
|
|
+ lineValue[2] > 1000) motors.setSpeeds(0, 0);
|
|
|
+ else Spurhalten();
|
|
|
}
|
|
|
- else
|
|
|
+ else //verfügbare Funktionen im Normalfall
|
|
|
{
|
|
|
- if(moveRate > 1000) CollisionDetection();
|
|
|
- else if(proxValue[0] == 6 || proxValue[1] == 6) motors.setSpeeds(0, 0);
|
|
|
- else if(proxValue[0] > 4 || proxValue[1] > 4) ObstacleAvoidance();
|
|
|
- else if((lineValue[0] > 500 && lineValue[0] < 1500) || (lineValue[2] > 500 && lineValue[2] < 1500)) Crossroad();
|
|
|
- else TrackKeeper();
|
|
|
+ if(moveRate > 1000) Kollisionserkennung();
|
|
|
+ else if(proxValue[1] == 6 || proxValue[2] == 6) motors.setSpeeds(0, 0);
|
|
|
+ else if(proxValue[1] == 5 || proxValue[2] == 5) Hindernisumfahren();
|
|
|
+ else if(lineValue[0] > 1000 || lineValue[2] > 1000) Abbiegen();
|
|
|
+ else Spurhalten();
|
|
|
}
|
|
|
|
|
|
//Ausgabe über Bluetoothverbindung
|