|
@@ -2,7 +2,7 @@
|
|
|
#line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
#line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
//Verfassser: Felix Stange, 4056379, MET2016
|
|
|
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 14.05.2018
|
|
|
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 17.05.2018
|
|
|
//Projektthema: Untersuchungen und Implementierung eines automatisierten Fahrens mittels Pololu Zumo
|
|
|
/*Kurzbeschreibung: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe
|
|
|
der Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen).
|
|
@@ -26,11 +26,13 @@ Zumo32U4Encoders encoders; //Motorencoder
|
|
|
LSM303 compass; //Beschleunigungssensor x-Achse
|
|
|
L3G gyro; //Drehbewegungssensor z-Achse
|
|
|
|
|
|
+//Medianfilter geben mittleren Wert einer Reihe mit ungerader Anzahl aus
|
|
|
MedianFilter LineFilter0(3, 0); //erstellen der Filter für Liniensensoren
|
|
|
-MedianFilter LineFilter1(3, 0); //Medianfilter geben mittleren Wert einer Reihe aus
|
|
|
-MedianFilter LineFilter2(3, 0); //Fenstergröße: 3, Basiswerte: 0 0 0
|
|
|
+MedianFilter LineFilter1(3, 0); //Fenstergröße: 3, Basiswerte: 0 0 0
|
|
|
+MedianFilter LineFilter2(3, 0);
|
|
|
+
|
|
|
MedianFilter ProxFilter0(5, 0); //erstellen der Filter für Abstandssensoren
|
|
|
-MedianFilter ProxFilter1(5, 0);
|
|
|
+MedianFilter ProxFilter1(5, 0); //Fenstergröße: 5, Basiswerte: 0 0 0 0 0
|
|
|
MedianFilter ProxFilter2(5, 0);
|
|
|
MedianFilter ProxFilter3(5, 0);
|
|
|
|
|
@@ -40,6 +42,7 @@ MedianFilter ProxFilter3(5, 0);
|
|
|
#define SIGN0 500
|
|
|
#define SIGN1 300
|
|
|
#define SIGN2 500
|
|
|
+
|
|
|
#define MAXSPEED 400
|
|
|
#define FULLSPEEDLEFT 104
|
|
|
#define HALFSPEEDLEFT 52
|
|
@@ -61,64 +64,66 @@ int32_t turnAngle = 0;
|
|
|
int16_t turnRate;
|
|
|
int16_t gyroOffset;
|
|
|
uint16_t gyroLastUpdate;
|
|
|
-
|
|
|
-//int32_t moveDisplay = 0; //Beschleunigung
|
|
|
-//int32_t moveDistance = 0;
|
|
|
-int16_t moveRate;
|
|
|
-int16_t compassOffset;
|
|
|
-//uint16_t compassLastUpdate;
|
|
|
+
|
|
|
+int16_t compassRate; //Beschleunigung
|
|
|
+int16_t compassOffset;
|
|
|
+int16_t compassLastUpdate;
|
|
|
|
|
|
uint16_t lastUpdate = 0; //Systemzeit
|
|
|
-uint16_t eventTime = 0; //Zeit seit letzter Geschwindigkeitsänderung
|
|
|
-char report[200]; //Ausgabe
|
|
|
+uint16_t eventTime = 0; //Zeitdifferenz
|
|
|
+uint16_t stopUpdate = 0; //Systemzeit
|
|
|
+uint16_t stopTime = 0; //Zeit seit letztem Manöver
|
|
|
float eventSpeed = 1; //vermindert die Geschwindigkeit bei Manövern
|
|
|
int btData = 0; //Gelesene Daten aus Bluetooth
|
|
|
-bool alarm = false; //zeigt Manöver abhängig von btData
|
|
|
+bool btBuffer = false; //puffert Daten von btData
|
|
|
+bool stop = false; //Sperrt Funktion Kontaktvermeiden
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
|
|
|
|
//Setup Bluetoothverbindung
|
|
|
-#line 78 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 81 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void BlueSetup();
|
|
|
-#line 88 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 91 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void LineSetup();
|
|
|
-#line 117 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 120 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void ProxSetup();
|
|
|
-#line 123 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 126 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void GyroSetup();
|
|
|
-#line 156 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 159 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void CompassSetup();
|
|
|
-#line 187 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 190 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void setup();
|
|
|
-#line 213 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 217 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void TimeUpdate();
|
|
|
-#line 219 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 224 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void StopUpdate();
|
|
|
+#line 230 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void LoopTiming();
|
|
|
-#line 263 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 274 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void SerielleAusgabe();
|
|
|
+#line 289 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void ProxUpdate();
|
|
|
-#line 294 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 316 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void LineUpdate();
|
|
|
-#line 304 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 326 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void GyroUpdate();
|
|
|
-#line 317 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 339 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void CompassUpdate();
|
|
|
-#line 330 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 348 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void EncoderUpdate();
|
|
|
-#line 341 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 359 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void Kontaktvermeiden();
|
|
|
-#line 360 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 393 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void Hindernisumfahren();
|
|
|
-#line 486 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 525 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void Abbiegen();
|
|
|
-#line 647 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 686 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void Spurhalten();
|
|
|
-#line 692 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 736 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void Spurfinden();
|
|
|
-#line 710 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
-void SerielleAusgabe();
|
|
|
-#line 725 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 757 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void loop();
|
|
|
-#line 78 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 81 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
void BlueSetup()
|
|
|
{
|
|
|
Serial1.begin(9600); //Initialisierung mit Datengeschwindigkeit(Baud)
|
|
@@ -217,7 +222,7 @@ void CompassSetup()
|
|
|
int32_t total = 0;
|
|
|
for (uint16_t i = 0; i < 1024; i++)
|
|
|
{
|
|
|
- compass.read();
|
|
|
+ compass.readAcc();
|
|
|
total += compass.a.x;
|
|
|
}
|
|
|
compassOffset = total / 1024;
|
|
@@ -248,18 +253,26 @@ void setup()
|
|
|
buttonA.waitForButton();
|
|
|
randomSeed(millis());
|
|
|
delay(500);
|
|
|
+ stopUpdate = millis();
|
|
|
//Serial1.println("Start");
|
|
|
}
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
|
|
|
|
-//Update Durchlaufzeit
|
|
|
+//Update Zeitdifferenz für alle Funktionen
|
|
|
void TimeUpdate()
|
|
|
{
|
|
|
uint16_t m = millis();
|
|
|
eventTime = m - lastUpdate;
|
|
|
}
|
|
|
|
|
|
+//Update Zeit für Kontaktvermeiden
|
|
|
+void StopUpdate()
|
|
|
+{
|
|
|
+ uint16_t m = millis();
|
|
|
+ stopTime = m - stopUpdate;
|
|
|
+}
|
|
|
+
|
|
|
void LoopTiming()
|
|
|
{
|
|
|
const int AL = 100; // Arraylänge, NUR GERADE Zahlen verwenden!
|
|
@@ -303,6 +316,21 @@ void LoopTiming()
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+//Funktion Serielle Ausgabe
|
|
|
+void SerielleAusgabe()
|
|
|
+{
|
|
|
+ char report[200];
|
|
|
+ snprintf_P(report, sizeof(report),
|
|
|
+ PSTR("Abstand: %d %d %d %d Linie: %d %d %d"),
|
|
|
+ proxValue[0], proxValue[1], proxValue[2], proxValue[3],
|
|
|
+ lineValue[0], lineValue[1], lineValue[2]);
|
|
|
+ Serial1.println(report);
|
|
|
+ snprintf_P(report, sizeof(report),
|
|
|
+ PSTR("Weg: %d %d Winkel: %d Beschleunigung: %d"),
|
|
|
+ driveRotation[0], driveRotation[1], rotationAngle, compassRate);
|
|
|
+ Serial1.println(report);
|
|
|
+}
|
|
|
+
|
|
|
//Update Abstandssensoren
|
|
|
void ProxUpdate()
|
|
|
{
|
|
@@ -320,10 +348,6 @@ void ProxUpdate()
|
|
|
proxValue[1] = ProxFilter1.in(proxSensors.countsFrontWithLeftLeds());
|
|
|
proxValue[2] = ProxFilter2.in(proxSensors.countsFrontWithRightLeds());
|
|
|
proxValue[3] = ProxFilter3.in(proxSensors.countsRightWithRightLeds());
|
|
|
- // Serial.println(proxValue[0]);
|
|
|
- // Serial.println(proxValue[1]);
|
|
|
- // Serial.println(proxValue[2]);
|
|
|
- // Serial.println(proxValue[3]);
|
|
|
done = true;
|
|
|
}
|
|
|
else
|
|
@@ -360,22 +384,18 @@ void GyroUpdate()
|
|
|
//Update Beschleunigungssensor
|
|
|
void CompassUpdate()
|
|
|
{
|
|
|
- compass.read(); //Rohwert 16384 entspricht 1g (9,81m/s²) bzw.
|
|
|
- moveRate = compass.a.x - compassOffset; //bei +/-2g Messbereich 0,61mg/LSB
|
|
|
- // uint16_t m = micros();
|
|
|
- // uint16_t dt = m - compassLastUpdate;
|
|
|
- // compassLastUpdate = m;
|
|
|
- // int32_t d = (int32_t)moveRate * dt;
|
|
|
- // moveDistance += (int64_t)d * dt >> 14;
|
|
|
- // moveDisplay = moveDistance * 9,81 / 1000;
|
|
|
+ compass.read(); //Rohwert 16384 entspricht 1g (9,81m/s²) bzw. bei +/-2g Messbereich 0,61mg/LSB
|
|
|
+ int16_t x = compass.a.x - compassOffset;
|
|
|
+ compassRate = x - compassLastUpdate;
|
|
|
+ compassLastUpdate = x;
|
|
|
}
|
|
|
|
|
|
//Update Encoder
|
|
|
void EncoderUpdate()
|
|
|
{
|
|
|
- encoderCounts[0] = encoders.getCountsLeft();
|
|
|
+ encoderCounts[0] += encoders.getCountsAndResetLeft();
|
|
|
driveRotation[0] = encoderCounts[0] / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
|
|
- encoderCounts[1] = encoders.getCountsRight();
|
|
|
+ encoderCounts[1] += encoders.getCountsAndResetRight();
|
|
|
driveRotation[1] = encoderCounts[1] / 910;
|
|
|
}
|
|
|
|
|
@@ -392,10 +412,25 @@ void Kontaktvermeiden()
|
|
|
while(proxValue[1] == 0 || proxValue[2] == 0)
|
|
|
{
|
|
|
ProxUpdate();
|
|
|
+ LineUpdate();
|
|
|
motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
|
|
|
- if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) break;
|
|
|
+ if(lineValue[0] > MARKLINE0 || lineValue[1] > MARKLINE1 || lineValue[2] > MARKLINE2) break;
|
|
|
+ }
|
|
|
+ lastUpdate = millis();
|
|
|
+ TimeUpdate();
|
|
|
+ while(eventTime < 1000)
|
|
|
+ {
|
|
|
+ TimeUpdate();
|
|
|
+ LineUpdate();
|
|
|
+ motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
|
|
|
+ if(lineValue[0] > MARKLINE0 || lineValue[1] > MARKLINE1 || lineValue[2] > MARKLINE2) break;
|
|
|
}
|
|
|
motors.setSpeeds(0, 0);
|
|
|
+ compassLastUpdate = 0;
|
|
|
+ compassRate = 0;
|
|
|
+ CompassUpdate();
|
|
|
+ stop = false;
|
|
|
+ stopUpdate = millis();
|
|
|
//Serial1.println("Vermeiden beendet");
|
|
|
Serial1.print(0);
|
|
|
}
|
|
@@ -426,14 +461,15 @@ void Hindernisumfahren()
|
|
|
motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
|
|
|
}
|
|
|
+
|
|
|
//geradeaus über Mittellinie fahren
|
|
|
LineUpdate();
|
|
|
while(lineValue[2] < MARKLINE2)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
- //if(lineValue[0] > MARKLINE0) break;
|
|
|
}
|
|
|
+
|
|
|
//rechts drehen
|
|
|
GyroUpdate();
|
|
|
while(rotationAngle > 10)
|
|
@@ -444,12 +480,13 @@ void Hindernisumfahren()
|
|
|
else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
else motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
|
|
|
}
|
|
|
+
|
|
|
//geradeaus fahren
|
|
|
motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
|
|
|
//Gegenverkehr beachten
|
|
|
ProxUpdate();
|
|
|
- //if(proxValue[1] < 5 || proxValue[2] < 5)
|
|
|
+ //if(proxValue[1] == 6 && proxValue[2] == 6)
|
|
|
//{
|
|
|
//Schritt 2: Hindernis passieren
|
|
|
//Serial1.println("Aufholen");
|
|
@@ -502,14 +539,15 @@ void Hindernisumfahren()
|
|
|
motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
|
|
|
if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
|
|
|
}
|
|
|
+
|
|
|
//geradeaus über Mittellinie fahren
|
|
|
LineUpdate();
|
|
|
while(lineValue[0] < MARKLINE0)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
- //if(lineValue[0] > MARKLINE0) break;
|
|
|
}
|
|
|
+
|
|
|
//links drehen
|
|
|
GyroUpdate();
|
|
|
while(rotationAngle < -10)
|
|
@@ -520,10 +558,13 @@ void Hindernisumfahren()
|
|
|
else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
}
|
|
|
+
|
|
|
//geradeaus fahren
|
|
|
//Serial1.println("Umfahren beendet");
|
|
|
- Serial1.print(0);
|
|
|
motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ stop = false;
|
|
|
+ stopUpdate = millis();
|
|
|
+ Serial1.print(0);
|
|
|
}
|
|
|
|
|
|
//Funktion Abbiegen
|
|
@@ -574,8 +615,9 @@ void Abbiegen()
|
|
|
while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
- motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10);
|
|
|
}
|
|
|
+
|
|
|
//links drehen
|
|
|
turnAngle = 0;
|
|
|
rotationAngle = 0;
|
|
@@ -588,6 +630,7 @@ void Abbiegen()
|
|
|
else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(HALFSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
}
|
|
|
+
|
|
|
//geradeaus fahren
|
|
|
motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
}
|
|
@@ -597,26 +640,22 @@ void Abbiegen()
|
|
|
{
|
|
|
//Serial1.println("links Abbiegen vom Rundkurs");
|
|
|
//links drehen
|
|
|
+ motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
turnAngle = 0;
|
|
|
rotationAngle = 0;
|
|
|
- driveRotation[1] = 0;
|
|
|
GyroUpdate();
|
|
|
while(rotationAngle < 40)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- EncoderUpdate();
|
|
|
motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
- //if(driveRotation[1] > 1) break;
|
|
|
}
|
|
|
- driveRotation[1] = 0;
|
|
|
GyroUpdate();
|
|
|
while(rotationAngle < 85)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- EncoderUpdate();
|
|
|
motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
- //if(driveRotation[1] > 1) break;
|
|
|
}
|
|
|
+
|
|
|
//geradeaus fahren
|
|
|
motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
lastUpdate = millis();
|
|
@@ -625,6 +664,7 @@ void Abbiegen()
|
|
|
{
|
|
|
TimeUpdate();
|
|
|
LineUpdate();
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
|
|
|
}
|
|
|
lastUpdate = millis();
|
|
@@ -642,14 +682,13 @@ void Abbiegen()
|
|
|
{
|
|
|
//Serial1.println("rechts Abbiegen");
|
|
|
//rechts drehen
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
|
|
|
turnAngle = 0;
|
|
|
rotationAngle = 0;
|
|
|
- driveRotation[0] = 0;
|
|
|
GyroUpdate();
|
|
|
while(rotationAngle > -40)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- EncoderUpdate();
|
|
|
LineUpdate();
|
|
|
motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
|
|
|
if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
|
|
@@ -683,6 +722,8 @@ void Abbiegen()
|
|
|
Spurhalten();
|
|
|
}
|
|
|
}
|
|
|
+ stop = false;
|
|
|
+ stopUpdate = millis();
|
|
|
//Serial1.println("Abbiegen beendet");
|
|
|
Serial1.print(0);
|
|
|
}
|
|
@@ -699,7 +740,7 @@ void Spurhalten()
|
|
|
{
|
|
|
ledYellow(1);
|
|
|
//Serial1.println("Spur nach rechts korrigieren");
|
|
|
- motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
|
|
|
+ motors.setSpeeds((int)FULLSPEEDLEFT/eventSpeed, (int)SLOWSPEEDRIGHT/eventSpeed);
|
|
|
while(Time < 100)
|
|
|
{
|
|
|
Update = millis();
|
|
@@ -707,6 +748,8 @@ void Spurhalten()
|
|
|
LineUpdate();
|
|
|
if(lineValue[2] > MARKLINE2) break;
|
|
|
}
|
|
|
+ stop = false;
|
|
|
+ stopUpdate = millis();
|
|
|
}
|
|
|
|
|
|
//rechte Linie erreicht, nach links fahren
|
|
@@ -714,7 +757,7 @@ void Spurhalten()
|
|
|
{
|
|
|
ledYellow(1);
|
|
|
//Serial1.println("Spur nach links korrigieren");
|
|
|
- motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);
|
|
|
+ motors.setSpeeds((int)SLOWSPEEDLEFT/eventSpeed, (int)FULLSPEEDRIGHT/eventSpeed);
|
|
|
while(Time < 100)
|
|
|
{
|
|
|
Update = millis();
|
|
@@ -722,13 +765,16 @@ void Spurhalten()
|
|
|
LineUpdate();
|
|
|
if(lineValue[0] > MARKLINE0) break;
|
|
|
}
|
|
|
+ stop = false;
|
|
|
+ stopUpdate = millis();
|
|
|
}
|
|
|
|
|
|
//normale Fahrt
|
|
|
else
|
|
|
{
|
|
|
ledGreen(1);
|
|
|
- motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);
|
|
|
+ motors.setSpeeds((int)FULLSPEEDLEFT/eventSpeed, (int)FULLSPEEDRIGHT/eventSpeed);
|
|
|
+ stop = true;
|
|
|
}
|
|
|
}
|
|
|
|
|
@@ -746,24 +792,12 @@ void Spurfinden()
|
|
|
motors.setSpeeds(-FULLSPEEDLEFT, -FULLSPEEDRIGHT);
|
|
|
if(eventTime > 3000) break;
|
|
|
}
|
|
|
+ stop = false;
|
|
|
+ stopUpdate = millis();
|
|
|
//Serial1.println("Spur gefunden");
|
|
|
Serial1.print(0);
|
|
|
}
|
|
|
|
|
|
-//Funktion Serielle Ausgabe
|
|
|
-void SerielleAusgabe()
|
|
|
-{
|
|
|
- snprintf_P(report, sizeof(report),
|
|
|
- PSTR("Abstand: %d %d %d %d Linie: %d %d %d"),
|
|
|
- proxValue[0], proxValue[1], proxValue[2], proxValue[3],
|
|
|
- lineValue[0], lineValue[1], lineValue[2]);
|
|
|
- Serial1.println(report);
|
|
|
- snprintf_P(report, sizeof(report),
|
|
|
- PSTR("Weg: %d %d Winkel: %d Beschleunigung: %d"),
|
|
|
- driveRotation[0], driveRotation[1], rotationAngle, moveRate);
|
|
|
- Serial1.println(report);
|
|
|
-}
|
|
|
-
|
|
|
/*-----------------------------------------------------------------*/
|
|
|
|
|
|
void loop()
|
|
@@ -780,27 +814,31 @@ void loop()
|
|
|
CompassUpdate();
|
|
|
LineUpdate();
|
|
|
TimeUpdate();
|
|
|
+ StopUpdate();
|
|
|
|
|
|
//Funktionsauswahl
|
|
|
- //btData = Serial1.readString();
|
|
|
if(Serial1.available() > 0) btData = Serial1.read();
|
|
|
- if(btData == '1') alarm = true;
|
|
|
- else if(btData == '0') alarm = false;
|
|
|
+ if(btData == '1') btBuffer = true;
|
|
|
+ else if(btData == '0') btBuffer = false;
|
|
|
+
|
|
|
//verfügbare Funktionen bei laufenden Manövern
|
|
|
- //if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kontaktvermeiden"|| btData == "Spurfinden")
|
|
|
- if(alarm == true)
|
|
|
+ if(btBuffer == true)
|
|
|
{
|
|
|
//Serial1.println("Verstanden");
|
|
|
eventSpeed = 1.4;
|
|
|
- if(moveRate > 1500 || (proxValue[1] == 6 && proxValue[2] == 6) || lineValue[0] > SIGN0 || lineValue[2] > SIGN2 || lineValue[1] > MARKLINE1) motors.setSpeeds(0, 0);
|
|
|
+ if(proxValue[0] == 6 || (proxValue[1] == 6 && proxValue[2] == 6)) motors.setSpeeds(0, 0);
|
|
|
+ else if(stop == true && stopTime > 2000 && abs(compassRate) > 3500) motors.setSpeeds(0, 0);
|
|
|
+ else if(lineValue[0] > SIGN0 || lineValue[2] > SIGN2 || lineValue[1] > MARKLINE1) motors.setSpeeds(0, 0);
|
|
|
else Spurhalten();
|
|
|
}
|
|
|
+
|
|
|
//verfügbare Funktionen im Normalfall
|
|
|
else
|
|
|
{
|
|
|
eventSpeed = 1.0;
|
|
|
- //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();
|
|
|
- if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
|
|
|
+ if(stop == true && stopTime > 2000 && abs(compassRate) > 3500) Kontaktvermeiden();
|
|
|
+ else if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
|
|
|
+ else if(lineValue[0] > 1600 && lineValue[2] > 1600) motors.setSpeeds(0, 0);
|
|
|
else if((lineValue[0] > SIGN0 && lineValue[0] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) Abbiegen();
|
|
|
else if((lineValue[2] > SIGN2 && lineValue[2] < 1600) && (lineValue[1] > SIGN1 && lineValue[1] < 800)) Abbiegen();
|
|
|
else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) Spurfinden();
|