|
@@ -1,5 +1,5 @@
|
|
|
//Verfassser: Felix Stange 4056379 MET2016
|
|
|
-//Datum: 19.07.2017 erstellt, 25.10.2017 zuletzt geändert
|
|
|
+//Datum: 19.07.2017 erstellt, 08.12.2017 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).
|
|
|
//Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es
|
|
@@ -42,10 +42,11 @@ int16_t moveRate;
|
|
|
int16_t compassOffset;
|
|
|
uint16_t compassLastUpdate;
|
|
|
|
|
|
+uint16_t LastUpdate;
|
|
|
int maxSpeed = 200; //Maximale Geschwindigkeit (möglich 400)
|
|
|
-char dir = "0"; //Fahrtrichtung, Ereignis
|
|
|
-char report[200]; //Ausgabe
|
|
|
-char attention; //zeigt Überhol-/Abbiegevorgang an
|
|
|
+char dir = 'O'; //Fahrtrichtung, Ereignis
|
|
|
+char report[250]; //Ausgabe
|
|
|
+int warning = 0; //1 zeigt Überhol-/Abbiegevorgang an
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
|
|
|
@@ -130,12 +131,13 @@ void setup()
|
|
|
{
|
|
|
//Initialisierung der Bluetoothverbindung
|
|
|
Serial1.begin(9600);
|
|
|
- if(Serial1.available()) Serial1.println("Bluetooth verfügbar");
|
|
|
+ if(Serial1.available()) Serial1.println("Verbindung hergestellt");
|
|
|
|
|
|
//Initialisierung und Kalibrierung der Sensoren
|
|
|
Serial1.println("Sensorkalibrierung");
|
|
|
Wire.begin();
|
|
|
delay(500);
|
|
|
+ LastUpdate = micros();
|
|
|
ProxSetup();
|
|
|
LineSetup();
|
|
|
GyroSetup();
|
|
@@ -151,6 +153,14 @@ void setup()
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
|
|
|
|
+//Update Durchlaufzeit
|
|
|
+void TimeUpdate()
|
|
|
+{
|
|
|
+ uint16_t m = micros();
|
|
|
+ uint16_t dt = m - LastUpdate;
|
|
|
+ LastUpdate = m;
|
|
|
+}
|
|
|
+
|
|
|
//Update Abstandssensoren
|
|
|
void ProxUpdate()
|
|
|
{
|
|
@@ -210,7 +220,7 @@ void EncoderUpdate()
|
|
|
//Funktion Kollisionserkennung
|
|
|
void CollisionDetection()
|
|
|
{
|
|
|
- dir = "X";
|
|
|
+ dir = 'X';
|
|
|
Serial1.println("Aufprall erkannt");
|
|
|
ledRed(1);
|
|
|
|
|
@@ -223,8 +233,9 @@ void CollisionDetection()
|
|
|
//Funktion Hindernisumfahrung
|
|
|
void ObstacleAvoidance()
|
|
|
{
|
|
|
- dir = "U";
|
|
|
- Serial1.println("Überholen");
|
|
|
+ dir = 'U';
|
|
|
+ Serial1.println("Überholen");
|
|
|
+ Serial1.println("1");
|
|
|
ledYellow(1);
|
|
|
|
|
|
//Schritt 1: Spurwechsel links
|
|
@@ -311,7 +322,7 @@ void TrackKeeper()
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
ledYellow(1);
|
|
|
delay(100);
|
|
|
- dir = "R";
|
|
|
+ dir = 'R';
|
|
|
}
|
|
|
|
|
|
//rechte Linie erreicht, nach links fahren
|
|
@@ -320,7 +331,7 @@ void TrackKeeper()
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
ledYellow(1);
|
|
|
delay(100);
|
|
|
- dir = "L";
|
|
|
+ dir = 'L';
|
|
|
}
|
|
|
|
|
|
//Linie überfahren, zurücksetzen
|
|
@@ -332,7 +343,7 @@ void TrackKeeper()
|
|
|
motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
}
|
|
|
delay(500);
|
|
|
- dir = "B";
|
|
|
+ dir = 'B';
|
|
|
}
|
|
|
|
|
|
//normale Fahrt
|
|
@@ -340,7 +351,7 @@ void TrackKeeper()
|
|
|
{
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
ledGreen(1);
|
|
|
- dir = "F";
|
|
|
+ dir = 'F';
|
|
|
}
|
|
|
}
|
|
|
|
|
@@ -348,8 +359,9 @@ void TrackKeeper()
|
|
|
void Crossroad()
|
|
|
{
|
|
|
ledYellow(1);
|
|
|
- dir = "A";
|
|
|
+ dir = 'A';
|
|
|
Serial1.println("Abbiegen");
|
|
|
+ Serial1.println("1");
|
|
|
|
|
|
//Kodierung analysieren
|
|
|
bool leftCode; //links => 1
|
|
@@ -386,7 +398,9 @@ void Crossroad()
|
|
|
while(rotationAngle != 90)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- motors.setSpeeds(0, maxSpeed/2);
|
|
|
+ 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);
|
|
|
}
|
|
|
//geradeaus fahren
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
@@ -401,7 +415,9 @@ void Crossroad()
|
|
|
while(rotationAngle != -90)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- motors.setSpeeds(maxSpeed/2, 0);
|
|
|
+ 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);
|
|
|
}
|
|
|
//geradeaus fahren
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
@@ -417,11 +433,11 @@ void Crossroad()
|
|
|
void SerialOutput()
|
|
|
{
|
|
|
snprintf_P(report, sizeof(report),
|
|
|
- PSTR("Abstand: %3d %3d %3d %3d Linie: %6d %6d %6d Richtung: %3c Weg: %6d %6d Winkel: %6d"),
|
|
|
+ PSTR("Abstand: %3d %3d %3d %3d Linie: %6d %6d %6d Weg: %3d %3d Winkel: %3d Ereignis: %3c Zeit: %3d"),
|
|
|
proxValue[0], proxValue[1], proxValue[2], proxValue[3],
|
|
|
lineValue[0], lineValue[1], lineValue[2],
|
|
|
- dir, driveRotation[0], driveRotation[1], rotationAngle);
|
|
|
- Serial1.println(report);
|
|
|
+ driveRotation[0], driveRotation[1], rotationAngle, dir, LastUpdate);
|
|
|
+ Serial.println(report);
|
|
|
}
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
@@ -433,6 +449,7 @@ void loop()
|
|
|
ledRed(0);
|
|
|
|
|
|
//Abfragen der Sensordaten
|
|
|
+ TimeUpdate();
|
|
|
LineUpdate();
|
|
|
ProxUpdate();
|
|
|
GyroUpdate();
|
|
@@ -440,11 +457,11 @@ void loop()
|
|
|
EncoderUpdate();
|
|
|
|
|
|
//Funktionsauswahl
|
|
|
- attention = Serial1.read();
|
|
|
- if(attention == "Überholen" || "Abbiegen")
|
|
|
+ warning = Serial1.read();
|
|
|
+ if(warning == 1)
|
|
|
{
|
|
|
maxSpeed = 100;
|
|
|
- if(moveRate > 1000 || proxValue[0] > 4 || proxValue[1] > 4 || lineValue[0] > 500 && lineValue[0] < 1500) ||
|
|
|
+ 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();
|
|
|
}
|