|
|
@@ -1,7 +1,7 @@
|
|
|
# 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
# 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 13.04.2018
|
|
|
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 04.05.2018
|
|
|
//Projektbeschreibung: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe
|
|
|
//der Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen).
|
|
|
//Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es
|
|
|
@@ -58,6 +58,7 @@ void BlueSetup()
|
|
|
Serial1.begin(9600); //Initialisierung mit Datengeschwindigkeit(Baud)
|
|
|
Serial1.setTimeout(10); //verkürzt Serial(1).read auf 10 ms statt 1000 ms
|
|
|
if(Serial1.available()) Serial.println("Bluetoothverbindung hergestellt");
|
|
|
+ if(Serial1.available() > 0) Serial1.read(); //Verwerfen der alten Informationen aus dem Puffer
|
|
|
}
|
|
|
|
|
|
//Setup Liniensensoren
|
|
|
@@ -66,27 +67,27 @@ void LineSetup()
|
|
|
ledYellow(1);
|
|
|
lineSensors.initThreeSensors(); //Initialisierung von 3 Sensoren (max 5)
|
|
|
|
|
|
- // //Kalibrierung
|
|
|
- // uint32_t total[3] = {0, 0, 0};
|
|
|
- // for(uint8_t i = 0; i < 120; i++)
|
|
|
- // {
|
|
|
- // if (i > 30 && i <= 90) motors.setSpeeds(FULLSPEEDLEFT, -FULLSPEEDRIGHT);
|
|
|
- // else motors.setSpeeds(-FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
- // lineSensors.read(lineOffset);
|
|
|
- // total[0] += lineOffset[0];
|
|
|
- // total[1] += lineOffset[1];
|
|
|
- // total[2] += lineOffset[2];
|
|
|
- lineSensors.calibrate();
|
|
|
- // }
|
|
|
- // motors.setSpeeds(0, 0);
|
|
|
- // lineOffset[0] = total[0] / 120;
|
|
|
- // lineOffset[1] = total[1] / 120;
|
|
|
- // lineOffset[2] = total[2] / 120;
|
|
|
+ //Kalibrierung
|
|
|
+ uint32_t total[3] = {0, 0, 0};
|
|
|
+ for(uint8_t i = 0; i < 120; i++)
|
|
|
+ {
|
|
|
+ if (i > 30 && i <= 90) motors.setSpeeds(106, -100);
|
|
|
+ else motors.setSpeeds(-106, 100);
|
|
|
+ lineSensors.read(lineOffset);
|
|
|
+ total[0] += lineOffset[0];
|
|
|
+ total[1] += lineOffset[1];
|
|
|
+ total[2] += lineOffset[2];
|
|
|
+ lineSensors.calibrate();
|
|
|
+ }
|
|
|
+ motors.setSpeeds(0, 0);
|
|
|
+ lineOffset[0] = total[0] / 120;
|
|
|
+ lineOffset[1] = total[1] / 120;
|
|
|
+ lineOffset[2] = total[2] / 120;
|
|
|
ledYellow(0);
|
|
|
|
|
|
- lineOffset[0] = 240;
|
|
|
- lineOffset[1] = 120;
|
|
|
- lineOffset[2] = 220;
|
|
|
+ // lineOffset[0] = 240;
|
|
|
+ // lineOffset[1] = 120;
|
|
|
+ // lineOffset[2] = 220;
|
|
|
}
|
|
|
|
|
|
//Setup Abstandssensoren
|
|
|
@@ -106,8 +107,8 @@ void GyroSetup()
|
|
|
ledRed(1);
|
|
|
while(1)
|
|
|
{
|
|
|
- //Serial1.println("Fehler Drehbewegungssensors");
|
|
|
- delay(1000);
|
|
|
+ Serial.println("Fehler Drehbewegungssensors");
|
|
|
+ delay(5000);
|
|
|
}
|
|
|
}
|
|
|
gyro.writeReg(L3G::CTRL1, 0b11111111); //Ausgaberate 800Hz, Tiefpassfilter 100Hz
|
|
|
@@ -139,8 +140,8 @@ void CompassSetup()
|
|
|
ledRed(1);
|
|
|
while(1)
|
|
|
{
|
|
|
- //Serial1.println("Fehler Beschleunigungssensors");
|
|
|
- delay(1000);
|
|
|
+ Serial.println("Fehler Beschleunigungssensors");
|
|
|
+ delay(5000);
|
|
|
}
|
|
|
}
|
|
|
compass.enableDefault();
|
|
|
@@ -296,27 +297,26 @@ void EncoderUpdate()
|
|
|
void Kontaktvermeiden()
|
|
|
{
|
|
|
//Serial1.println("Kontaktvermeiden");
|
|
|
- Serial1.println(1);
|
|
|
+ Serial1.print(1);
|
|
|
ledRed(1);
|
|
|
-
|
|
|
motors.setSpeeds(0, 0);
|
|
|
- delay(500);
|
|
|
+ delay(1000);
|
|
|
while(proxValue[1] == 0 || proxValue[2] == 0)
|
|
|
{
|
|
|
ProxUpdate();
|
|
|
- motors.setSpeeds(-54, -50);
|
|
|
- if(lineValue[0] > 150 || lineValue[2] > 170) break;
|
|
|
+ motors.setSpeeds(-53, -50);
|
|
|
+ if(lineValue[0] > 150 || lineValue[2] > 120) break;
|
|
|
}
|
|
|
- delay(500);
|
|
|
+ motors.setSpeeds(0, 0);
|
|
|
//Serial1.println("Vermeiden beendet");
|
|
|
- Serial1.println(0);
|
|
|
+ Serial1.print(0);
|
|
|
}
|
|
|
|
|
|
//Funktion Hindernisumfahrung
|
|
|
void Hindernisumfahren()
|
|
|
{
|
|
|
//Serial1.println("Hindernisumfahren");
|
|
|
- Serial1.println(1);
|
|
|
+ Serial1.print(1);
|
|
|
ledYellow(1);
|
|
|
|
|
|
//Schritt 1: Spurwechsel links
|
|
|
@@ -329,14 +329,14 @@ void Hindernisumfahren()
|
|
|
GyroUpdate();
|
|
|
LineUpdate();
|
|
|
motors.setSpeeds(27, 100);
|
|
|
- if(lineValue[2] > 170 && lineValue[2] < 450) break;
|
|
|
+ if(lineValue[2] > 120 && lineValue[2] < 500) break;
|
|
|
}
|
|
|
//geradeaus über Mittellinie fahren
|
|
|
LineUpdate();
|
|
|
- while(lineValue[2] < 170)
|
|
|
+ while(lineValue[2] < 120)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
- motors.setSpeeds(107, 100);
|
|
|
+ motors.setSpeeds(106, 100);
|
|
|
//if(lineValue[0] > MARKLINE0) break;
|
|
|
}
|
|
|
//rechts drehen
|
|
|
@@ -345,12 +345,12 @@ void Hindernisumfahren()
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
LineUpdate();
|
|
|
- if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 0);
|
|
|
- else if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(107, 100);
|
|
|
- else motors.setSpeeds(107, 25);
|
|
|
+ if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(106, 0);
|
|
|
+ else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(106, 100);
|
|
|
+ else motors.setSpeeds(106, 25);
|
|
|
}
|
|
|
//geradeaus fahren
|
|
|
- motors.setSpeeds(107, 100);
|
|
|
+ motors.setSpeeds(106, 100);
|
|
|
|
|
|
//Gegenverkehr beachten
|
|
|
ProxUpdate();
|
|
|
@@ -395,15 +395,15 @@ void Hindernisumfahren()
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
LineUpdate();
|
|
|
- motors.setSpeeds(107, 25);
|
|
|
- if(lineValue[0] > 150 && lineValue[0] < 400) break;
|
|
|
+ motors.setSpeeds(106, 25);
|
|
|
+ if(lineValue[0] > 150 && lineValue[0] < 500) break;
|
|
|
}
|
|
|
//geradeaus über Mittellinie fahren
|
|
|
LineUpdate();
|
|
|
while(lineValue[0] < 150)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
- motors.setSpeeds(107, 100);
|
|
|
+ motors.setSpeeds(106, 100);
|
|
|
//if(lineValue[0] > MARKLINE0) break;
|
|
|
}
|
|
|
//links drehen
|
|
|
@@ -412,14 +412,14 @@ void Hindernisumfahren()
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
LineUpdate();
|
|
|
- if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(0, 100);
|
|
|
- else if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 100);
|
|
|
+ if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(0, 100);
|
|
|
+ else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(106, 100);
|
|
|
else motors.setSpeeds(27, 100);
|
|
|
}
|
|
|
//geradeaus fahren
|
|
|
//Serial1.println("Umfahren beendet");
|
|
|
- Serial1.println(0);
|
|
|
- motors.setSpeeds(107, 100);
|
|
|
+ Serial1.print(0);
|
|
|
+ motors.setSpeeds(106, 100);
|
|
|
}
|
|
|
|
|
|
//Funktion Abbiegen
|
|
|
@@ -427,70 +427,65 @@ void Abbiegen()
|
|
|
{
|
|
|
ledYellow(1);
|
|
|
//Serial1.println("Abbiegen");
|
|
|
- Serial1.println(1);
|
|
|
+ Serial1.print(1);
|
|
|
|
|
|
//Markierung analysieren
|
|
|
LineUpdate();
|
|
|
bool leftCode; //links => 1
|
|
|
bool rightCode; //rechts => 2
|
|
|
- if(lineValue[0] > 400) leftCode = true;
|
|
|
+ if((lineValue[0] > 500 && lineValue[0] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) leftCode = true;
|
|
|
else leftCode = false;
|
|
|
- if(lineValue[2] > 450) rightCode = true;
|
|
|
+ if((lineValue[2] > 500 && lineValue[2] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) rightCode = true;
|
|
|
else rightCode = false;
|
|
|
|
|
|
//Zufallszahl erzeugen
|
|
|
uint8_t randy; //geradeaus => 0
|
|
|
if(leftCode == true && rightCode == true) randy = random(1, 3); //1, 2
|
|
|
- else if(leftCode == true && rightCode == false) randy = 1;
|
|
|
- // else if(leftCode == true && rightCode == false)
|
|
|
- // {
|
|
|
- // randy = random(0, 2); //0, 1
|
|
|
- // if(randy == 0) randy = random(0, 2); //erhöht Wahrscheinlickeit abzubiegen
|
|
|
- // }
|
|
|
- else if(leftCode == false && rightCode == true) randy = 2;
|
|
|
- // else if(leftCode == false && rightCode == true)
|
|
|
- // {
|
|
|
- // randy = random(3); //0, (1), 2
|
|
|
- // if(randy == 0) randy = random(3); //erhöht Wahrscheinlickeit abzubiegen
|
|
|
- // while(randy == 1) randy = random(3); //!1 => 0, 2
|
|
|
- // }
|
|
|
+ else if(leftCode == true && rightCode == false) //randy = 1;
|
|
|
+ {
|
|
|
+ randy = random(0, 2); //0, 1
|
|
|
+ //if(randy == 0) randy = random(0, 2); //erhöht Wahrscheinlickeit abzubiegen
|
|
|
+ }
|
|
|
+ else if(leftCode == false && rightCode == true) //randy = 2;
|
|
|
+ {
|
|
|
+ randy = random(0, 2); //0, 1
|
|
|
+ //if(randy == 0) randy = random(0, 2); //erhöht Wahrscheinlickeit abzubiegen
|
|
|
+ if(randy == 1) randy = 2; //!1 => 2
|
|
|
+ }
|
|
|
|
|
|
//links Abbiegen (von der Verbindungsstrecke)
|
|
|
if(randy == 1 && rightCode == true)
|
|
|
{
|
|
|
//Serial1.println("links Abbiegen von der Verbindungsstrecke");
|
|
|
//geradeaus zur Mittellinie fahren
|
|
|
- motors.setSpeeds(107, 100 +10);
|
|
|
+ motors.setSpeeds(106, 100 +10);
|
|
|
lastUpdate = millis();
|
|
|
TimeUpdate();
|
|
|
while(eventTime < 300)
|
|
|
{
|
|
|
TimeUpdate();
|
|
|
- //Serial1.println(eventTime);
|
|
|
- motors.setSpeeds(107, 100 +10);
|
|
|
+ motors.setSpeeds(106, 100 +10);
|
|
|
}
|
|
|
LineUpdate();
|
|
|
- //Serial1.println("Linie suchen");
|
|
|
- while(lineValue[0] < 150 && lineValue[2] < 170)
|
|
|
+ while(lineValue[0] < 150 && lineValue[2] < 120)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
- motors.setSpeeds(107, 100);
|
|
|
+ motors.setSpeeds(106, 100);
|
|
|
}
|
|
|
//links drehen
|
|
|
turnAngle = 0;
|
|
|
rotationAngle = 0;
|
|
|
GyroUpdate();
|
|
|
- //Serial1.println("Drehen");
|
|
|
while(rotationAngle < 90)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
LineUpdate();
|
|
|
- if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(0, 100);
|
|
|
- else if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 100);
|
|
|
+ if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(0, 100);
|
|
|
+ else if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(53, 100);
|
|
|
else motors.setSpeeds(27, 100);
|
|
|
}
|
|
|
//geradeaus fahren
|
|
|
- motors.setSpeeds(107, 100);
|
|
|
+ motors.setSpeeds(106, 100);
|
|
|
}
|
|
|
|
|
|
//links Abbiegen (vom Rundkurs)
|
|
|
@@ -507,7 +502,7 @@ void Abbiegen()
|
|
|
GyroUpdate();
|
|
|
EncoderUpdate();
|
|
|
motors.setSpeeds(27, 100);
|
|
|
- if(driveRotation[1] > 1) break;
|
|
|
+ //if(driveRotation[1] > 1) break;
|
|
|
}
|
|
|
driveRotation[1] = 0;
|
|
|
GyroUpdate();
|
|
|
@@ -516,23 +511,32 @@ void Abbiegen()
|
|
|
GyroUpdate();
|
|
|
EncoderUpdate();
|
|
|
motors.setSpeeds(27, 100);
|
|
|
- if(driveRotation[1] > 1) break;
|
|
|
+ //if(driveRotation[1] > 1) break;
|
|
|
}
|
|
|
//geradeaus fahren
|
|
|
- motors.setSpeeds(107, 100);
|
|
|
+ motors.setSpeeds(106, 100);
|
|
|
lastUpdate = millis();
|
|
|
TimeUpdate();
|
|
|
while(eventTime < 1000)
|
|
|
{
|
|
|
TimeUpdate();
|
|
|
LineUpdate();
|
|
|
- if(lineValue[2] > 170 && lineValue[2] < 450) break;
|
|
|
+ if(lineValue[2] > 120 && lineValue[2] < 500) break;
|
|
|
+ }
|
|
|
+ lastUpdate = millis();
|
|
|
+ TimeUpdate();
|
|
|
+ while(eventTime < 1000)
|
|
|
+ {
|
|
|
+ TimeUpdate();
|
|
|
+ LineUpdate();
|
|
|
+ Spurhalten();
|
|
|
}
|
|
|
}
|
|
|
|
|
|
//rechts Abbiegen
|
|
|
else if(randy == 2 && rightCode == true)
|
|
|
{
|
|
|
+ //Serial1.println("rechts Abbiegen");
|
|
|
//rechts drehen
|
|
|
turnAngle = 0;
|
|
|
rotationAngle = 0;
|
|
|
@@ -543,8 +547,8 @@ void Abbiegen()
|
|
|
GyroUpdate();
|
|
|
EncoderUpdate();
|
|
|
LineUpdate();
|
|
|
- motors.setSpeeds(107, 25);
|
|
|
- if(driveRotation[0] > 1 || (lineValue[0] > 150 && lineValue[0] < 400)) break;
|
|
|
+ motors.setSpeeds(106, 25);
|
|
|
+ if(lineValue[0] > 150 && lineValue[0] < 500) break;
|
|
|
}
|
|
|
GyroUpdate();
|
|
|
lastUpdate = millis();
|
|
|
@@ -554,17 +558,18 @@ void Abbiegen()
|
|
|
GyroUpdate();
|
|
|
LineUpdate();
|
|
|
if(eventTime > 3000) break;
|
|
|
- if(lineValue[0] > 150 && lineValue[0] < 400) motors.setSpeeds(107, 0);
|
|
|
+ if(lineValue[0] > 150 && lineValue[0] < 500) motors.setSpeeds(106, 0);
|
|
|
//else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) motors.setSpeeds(-SLOWSPEEDLEFT, -SLOWSPEEDRIGHT);
|
|
|
- else if(lineValue[2] > 170 && lineValue[2] < 450) motors.setSpeeds(107, 50);
|
|
|
- else motors.setSpeeds(107, 25);
|
|
|
+ else if(lineValue[2] > 120 && lineValue[2] < 500) motors.setSpeeds(106, 50);
|
|
|
+ else motors.setSpeeds(106, 25);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
//nicht Abbiegen, geradeaus fahren
|
|
|
else
|
|
|
{
|
|
|
- motors.setSpeeds(107, 100);
|
|
|
+ //Serial1.println("nicht Abbiegen");
|
|
|
+ motors.setSpeeds(106, 100);
|
|
|
lastUpdate = millis();
|
|
|
TimeUpdate();
|
|
|
while(eventTime < 1000)
|
|
|
@@ -575,57 +580,70 @@ void Abbiegen()
|
|
|
}
|
|
|
}
|
|
|
//Serial1.println("Abbiegen beendet");
|
|
|
- Serial1.println(0);
|
|
|
+ Serial1.print(0);
|
|
|
}
|
|
|
|
|
|
//Funktion Spurhalten
|
|
|
void Spurhalten()
|
|
|
{
|
|
|
//linke Linie erreicht, nach rechts fahren
|
|
|
- if(lineValue[0] > 150 && lineValue[0] < 400)
|
|
|
+ if(lineValue[0] > 150 && lineValue[0] < 500)
|
|
|
{
|
|
|
ledYellow(1);
|
|
|
//Serial1.println("Spur nach rechts korrigieren");
|
|
|
- motors.setSpeeds(107/eventSpeed, 25/eventSpeed);
|
|
|
- delay(100);
|
|
|
+ motors.setSpeeds(106/eventSpeed, 25/eventSpeed);
|
|
|
+ lastUpdate = millis();
|
|
|
+ TimeUpdate();
|
|
|
+ while(eventTime < 100)
|
|
|
+ {
|
|
|
+ TimeUpdate();
|
|
|
+ LineUpdate();
|
|
|
+ if(lineValue[2] > 120) break;
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
//rechte Linie erreicht, nach links fahren
|
|
|
- else if(lineValue[2] > 170 && lineValue[2] < 450)
|
|
|
+ else if(lineValue[2] > 120 && lineValue[2] < 500)
|
|
|
{
|
|
|
ledYellow(1);
|
|
|
//Serial1.println("Spur nach links korrigieren");
|
|
|
motors.setSpeeds(27/eventSpeed, 100/eventSpeed);
|
|
|
- delay(100);
|
|
|
- }
|
|
|
-
|
|
|
- //Linie überfahren, zurücksetzen
|
|
|
- else if(lineValue[1] > 100 && lineValue[1] < 350)
|
|
|
- {
|
|
|
- ledRed(1);
|
|
|
- //Serial1.println("Spurfinden");
|
|
|
- Serial1.println(1);
|
|
|
lastUpdate = millis();
|
|
|
- while(lineValue[0] < 150 && lineValue[2] < 170) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
|
|
|
+ TimeUpdate();
|
|
|
+ while(eventTime < 100)
|
|
|
{
|
|
|
TimeUpdate();
|
|
|
LineUpdate();
|
|
|
- motors.setSpeeds(-107/eventSpeed, -100/eventSpeed);
|
|
|
- if(eventTime > 3000) break;
|
|
|
+ if(lineValue[0] > 150) break;
|
|
|
}
|
|
|
- delay(100);
|
|
|
- //Serial1.println("Spur gefunden");
|
|
|
- Serial1.println(0);
|
|
|
}
|
|
|
|
|
|
//normale Fahrt
|
|
|
else
|
|
|
{
|
|
|
ledGreen(1);
|
|
|
- motors.setSpeeds(107/eventSpeed, 100/eventSpeed);
|
|
|
+ motors.setSpeeds(106/eventSpeed, 100/eventSpeed);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+//Funktion Spurfinden
|
|
|
+void Spurfinden()
|
|
|
+{
|
|
|
+ ledRed(1);
|
|
|
+ //Serial1.println("Spurfinden");
|
|
|
+ Serial1.print(1);
|
|
|
+ lastUpdate = millis();
|
|
|
+ while(lineValue[0] < 150 && lineValue[2] < 120) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
|
|
|
+ {
|
|
|
+ TimeUpdate();
|
|
|
+ LineUpdate();
|
|
|
+ motors.setSpeeds(-106, -100);
|
|
|
+ if(eventTime > 3000) break;
|
|
|
+ }
|
|
|
+ //Serial1.println("Spur gefunden");
|
|
|
+ Serial1.print(0);
|
|
|
+}
|
|
|
+
|
|
|
//Funktion Serielle Ausgabe
|
|
|
void SerielleAusgabe()
|
|
|
{
|
|
|
@@ -644,17 +662,17 @@ void SerielleAusgabe()
|
|
|
|
|
|
void loop()
|
|
|
{
|
|
|
- //LoopTiming(); //Zykluszeit beträgt max. 30ms im Idle
|
|
|
+ //LoopTiming(); //Zykluszeit beträgt max. 20ms im Idle
|
|
|
ledGreen(0);
|
|
|
ledYellow(0);
|
|
|
ledRed(0);
|
|
|
|
|
|
- //Abfragen der Sensordaten
|
|
|
- LineUpdate();
|
|
|
+ //Abfragen der Sensordaten
|
|
|
ProxUpdate();
|
|
|
EncoderUpdate();
|
|
|
GyroUpdate();
|
|
|
CompassUpdate();
|
|
|
+ LineUpdate();
|
|
|
TimeUpdate();
|
|
|
|
|
|
//Funktionsauswahl
|
|
|
@@ -667,18 +685,20 @@ void loop()
|
|
|
if(alarm == true)
|
|
|
{
|
|
|
//Serial1.println("Verstanden");
|
|
|
- eventSpeed = 2;
|
|
|
- if(moveRate > 1000 || proxValue[1] == 6 || proxValue[2] == 6 || lineValue[0] > 400 || lineValue[2] > 450) motors.setSpeeds(0, 0);
|
|
|
+ eventSpeed = 1.4;
|
|
|
+ if(moveRate > 1500 || (proxValue[1] == 6 && proxValue[2] == 6) || lineValue[0] > 500 || lineValue[2] > 500 || lineValue[1] > 100) motors.setSpeeds(0, 0);
|
|
|
else Spurhalten();
|
|
|
}
|
|
|
//verfügbare Funktionen im Normalfall
|
|
|
else
|
|
|
{
|
|
|
- eventSpeed = 1;
|
|
|
+ eventSpeed = 1.0;
|
|
|
//if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();
|
|
|
- if(proxValue[2] == 6) Hindernisumfahren();
|
|
|
- else if(lineValue[0] > 400 && lineValue[0] < 1000 || lineValue[2] > 450 && lineValue[2] < 1000) Abbiegen();
|
|
|
+ if(proxValue[1] == 6 && proxValue[2] == 6) Hindernisumfahren();
|
|
|
+ else if((lineValue[0] > 500 && lineValue[0] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) Abbiegen();
|
|
|
+ else if((lineValue[2] > 500 && lineValue[2] < 1400) && (lineValue[1] > 300 && lineValue[1] < 700)) Abbiegen();
|
|
|
+ else if(lineValue[1] > 100 && lineValue[1] < 300) Spurfinden();
|
|
|
else Spurhalten();
|
|
|
}
|
|
|
- //LoopTiming();
|
|
|
+ //LoopTiming();
|
|
|
}
|
fstange