|
|
@@ -1,5 +1,5 @@
|
|
|
//Verfassser: Felix Stange, 4056379, MET2016
|
|
|
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 26.03.2018
|
|
|
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 13.04.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
|
|
|
@@ -21,14 +21,19 @@ Zumo32U4Encoders encoders; //Motorencoder
|
|
|
LSM303 compass; //Beschleunigungssensor x-Achse
|
|
|
L3G gyro; //Drehbewegungssensor z-Achse
|
|
|
|
|
|
-#define MARKLINE0 200
|
|
|
+#define MARKLINE0 150
|
|
|
#define MARKLINE1 100
|
|
|
-#define MARKLINE2 200
|
|
|
-#define SIGN 1000
|
|
|
+#define MARKLINE2 170
|
|
|
+#define SIGN0 400
|
|
|
+#define SIGN1 350
|
|
|
+#define SIGN2 450
|
|
|
#define MAXSPEED 400
|
|
|
-#define FULLSPEED 100
|
|
|
-#define HALFSPEED 50
|
|
|
-#define SLOWSPEED 25
|
|
|
+#define FULLSPEEDLEFT 107
|
|
|
+#define HALFSPEEDLEFT 54
|
|
|
+#define SLOWSPEEDLEFT 27
|
|
|
+#define FULLSPEEDRIGHT 100
|
|
|
+#define HALFSPEEDRIGHT 50
|
|
|
+#define SLOWSPEEDRIGHT 25
|
|
|
|
|
|
int16_t lineValue[3]; //Liniensensoren
|
|
|
uint16_t lineOffset[3]; //von links (0) nach rechts (2)
|
|
|
@@ -53,35 +58,47 @@ int16_t compassOffset;
|
|
|
uint16_t lastUpdate = 0; //Systemzeit
|
|
|
uint16_t eventTime = 0; //Zeit seit letzter Geschwindigkeitsänderung
|
|
|
char report[200]; //Ausgabe
|
|
|
-int eventSpeed = 1; //vermindert die Geschwindigkeit bei Manövern
|
|
|
-char dir; //Fahrtrichtung, Ereignis
|
|
|
-String btData; //Gelesene Daten aus Bluetooth
|
|
|
+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
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
|
|
|
|
+//Setup Bluetoothverbindung
|
|
|
+void BlueSetup()
|
|
|
+{
|
|
|
+ Serial1.begin(9600); //Initialisierung mit Datengeschwindigkeit(Baud)
|
|
|
+ Serial1.setTimeout(10); //verkürzt Serial(1).read auf 10 ms statt 1000 ms
|
|
|
+ if(Serial.available()) Serial.println("Bluetoothverbindung hergestellt");
|
|
|
+}
|
|
|
+
|
|
|
//Setup Liniensensoren
|
|
|
void LineSetup()
|
|
|
{
|
|
|
ledYellow(1);
|
|
|
- lineSensors.initThreeSensors();
|
|
|
+ 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(FULLSPEED, -FULLSPEED);
|
|
|
- else motors.setSpeeds(-FULLSPEED, FULLSPEED);
|
|
|
- lineSensors.read(lineOffset);
|
|
|
- total[0] += lineOffset[0];
|
|
|
- total[1] += lineOffset[1];
|
|
|
- total[2] += lineOffset[2];
|
|
|
+ // //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;
|
|
|
ledYellow(0);
|
|
|
- motors.setSpeeds(0, 0);
|
|
|
- lineOffset[0] = total[0] / 120;
|
|
|
- lineOffset[1] = total[1] / 120;
|
|
|
- lineOffset[2] = total[2] / 120;
|
|
|
+
|
|
|
+ lineOffset[0] = 240;
|
|
|
+ lineOffset[1] = 120;
|
|
|
+ lineOffset[2] = 220;
|
|
|
}
|
|
|
|
|
|
//Setup Abstandssensoren
|
|
|
@@ -94,15 +111,15 @@ void ProxSetup()
|
|
|
void GyroSetup()
|
|
|
{
|
|
|
ledYellow(1);
|
|
|
- gyro.init();
|
|
|
- if (!gyro.init())
|
|
|
+ gyro.init(); //Initialisierung
|
|
|
+ if(!gyro.init()) //Fehlerabfrage
|
|
|
{
|
|
|
//Fehler beim Initialisieren des Drehbewegungssensors
|
|
|
ledRed(1);
|
|
|
while(1)
|
|
|
{
|
|
|
- Serial1.println(F("Fehler Drehbewegungssensors"));
|
|
|
- delay(100);
|
|
|
+ //Serial1.println("Fehler Drehbewegungssensors");
|
|
|
+ delay(1000);
|
|
|
}
|
|
|
}
|
|
|
gyro.writeReg(L3G::CTRL1, 0b11111111); //Ausgaberate 800Hz, Tiefpassfilter 100Hz
|
|
|
@@ -111,7 +128,7 @@ void GyroSetup()
|
|
|
|
|
|
//Kalibrierung
|
|
|
int32_t total = 0;
|
|
|
- for (uint16_t i = 0; i < 1024; i++)
|
|
|
+ for(uint16_t i = 0; i < 1024; i++)
|
|
|
{
|
|
|
while(!gyro.readReg(L3G::STATUS_REG) & 0x08); //Fehlerabfrage
|
|
|
gyro.read();
|
|
|
@@ -127,15 +144,15 @@ void GyroSetup()
|
|
|
void CompassSetup()
|
|
|
{
|
|
|
ledYellow(1);
|
|
|
- compass.init();
|
|
|
- if (!compass.init())
|
|
|
+ compass.init(); //Initialisierung
|
|
|
+ if(!compass.init()) //Fehlerabfrage
|
|
|
{
|
|
|
//Fehler beim Initialisieren des Beschleunigungssensors
|
|
|
ledRed(1);
|
|
|
while(1)
|
|
|
{
|
|
|
- Serial.println(F("Fehler Beschleunigungssensors"));
|
|
|
- delay(100);
|
|
|
+ Serial1.println("Fehler Beschleunigungssensors");
|
|
|
+ delay(1000);
|
|
|
}
|
|
|
}
|
|
|
compass.enableDefault();
|
|
|
@@ -158,12 +175,10 @@ void CompassSetup()
|
|
|
void setup()
|
|
|
{
|
|
|
//Initialisierung der Bluetoothverbindung
|
|
|
- Serial1.begin(9600);
|
|
|
- Serial1.setTimeout(10); //verkürzt Serial.readString auf 10 ms statt 1s
|
|
|
- if(Serial1.available()) Serial1.println("Verbindung hergestellt");
|
|
|
+ BlueSetup();
|
|
|
|
|
|
//Initialisierung und Kalibrierung der Sensoren
|
|
|
- Serial1.println("Sensorkalibrierung");
|
|
|
+ //Serial1.println("Sensorkalibrierung");
|
|
|
Wire.begin();
|
|
|
delay(500);
|
|
|
ProxSetup();
|
|
|
@@ -172,13 +187,12 @@ void setup()
|
|
|
CompassSetup();
|
|
|
|
|
|
//Zumo bereit zu starten
|
|
|
- Serial1.println("Zumo bereit, starte mit A");
|
|
|
+ //Serial1.println("Zumo bereit, starte mit A");
|
|
|
ledGreen(1);
|
|
|
buttonA.waitForButton();
|
|
|
randomSeed(millis());
|
|
|
delay(500);
|
|
|
- motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
- lastUpdate = millis();
|
|
|
+ //Serial1.println("Start");
|
|
|
}
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
|
@@ -271,12 +285,12 @@ 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;
|
|
|
+ // 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;
|
|
|
}
|
|
|
|
|
|
//Update Encoder
|
|
|
@@ -293,8 +307,8 @@ void EncoderUpdate()
|
|
|
//Funktion Kontaktvermeiden
|
|
|
void Kontaktvermeiden()
|
|
|
{
|
|
|
- dir = 'X';
|
|
|
- Serial1.println("Kontaktvermeiden");
|
|
|
+ //Serial1.println("Kontaktvermeiden");
|
|
|
+ Serial1.println(1);
|
|
|
ledRed(1);
|
|
|
|
|
|
motors.setSpeeds(0, 0);
|
|
|
@@ -302,140 +316,229 @@ void Kontaktvermeiden()
|
|
|
while(proxValue[1] == 0 || proxValue[2] == 0)
|
|
|
{
|
|
|
ProxUpdate();
|
|
|
- motors.setSpeeds(-HALFSPEED, -HALFSPEED);
|
|
|
- if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) return;
|
|
|
+ motors.setSpeeds(-HALFSPEEDLEFT, -HALFSPEEDRIGHT);
|
|
|
+ if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) break;
|
|
|
}
|
|
|
delay(500);
|
|
|
-
|
|
|
- Serial1.println("Vermeiden beendet");
|
|
|
- motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
- lastUpdate = millis();
|
|
|
+ //Serial1.println("Vermeiden beendet");
|
|
|
+ Serial1.println(0);
|
|
|
}
|
|
|
|
|
|
//Funktion Hindernisumfahrung
|
|
|
void Hindernisumfahren()
|
|
|
{
|
|
|
- dir = 'U';
|
|
|
- Serial1.println("Hindernisumfahren");
|
|
|
+ //Serial1.println("Hindernisumfahren");
|
|
|
+ Serial1.println(1);
|
|
|
ledYellow(1);
|
|
|
|
|
|
//Schritt 1: Spurwechsel links
|
|
|
- //links drehen
|
|
|
+ //links drehen
|
|
|
+ turnAngle = 0;
|
|
|
rotationAngle = 0;
|
|
|
GyroUpdate();
|
|
|
while(rotationAngle < 45)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- motors.setSpeeds(HALFSPEED, FULLSPEED);
|
|
|
+ LineUpdate();
|
|
|
+ motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
|
|
|
}
|
|
|
//geradeaus über Mittellinie fahren
|
|
|
LineUpdate();
|
|
|
while(lineValue[2] < MARKLINE2)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
- motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ //if(lineValue[0] > MARKLINE0) break;
|
|
|
}
|
|
|
//rechts drehen
|
|
|
GyroUpdate();
|
|
|
- while(rotationAngle > 0)
|
|
|
+ while(rotationAngle > 10)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- motors.setSpeeds(FULLSPEED, HALFSPEED);
|
|
|
+ LineUpdate();
|
|
|
+ if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, 0);
|
|
|
+ else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ else motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
|
|
|
}
|
|
|
//geradeaus fahren
|
|
|
- motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
|
|
|
//Gegenverkehr beachten
|
|
|
ProxUpdate();
|
|
|
- if(proxValue[1] < 5 || proxValue[2] < 5)
|
|
|
- {
|
|
|
+ //if(proxValue[1] < 5 || proxValue[2] < 5)
|
|
|
+ //{
|
|
|
//Schritt 2: Hindernis passieren
|
|
|
- motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
- delay(1000);
|
|
|
+ //Serial1.println("Aufholen");
|
|
|
+ lastUpdate = millis();
|
|
|
+ while(proxValue[3] < 6 && eventTime < 3000)
|
|
|
+ {
|
|
|
+ TimeUpdate();
|
|
|
+ ProxUpdate();
|
|
|
+ LineUpdate();
|
|
|
+ Spurhalten();
|
|
|
+ }
|
|
|
+ //Serial1.println("Überholen");
|
|
|
ProxUpdate();
|
|
|
- while(proxValue[3] > 4)
|
|
|
+ while(proxValue[3] == 6)
|
|
|
{
|
|
|
- motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
ProxUpdate();
|
|
|
+ LineUpdate();
|
|
|
Spurhalten();
|
|
|
}
|
|
|
- }
|
|
|
+ //Serial1.println("Abstand gewinnen");
|
|
|
+ lastUpdate = millis();
|
|
|
+ TimeUpdate();
|
|
|
+ while(eventTime < 3000)
|
|
|
+ {
|
|
|
+ //Serial1.println(eventTime);
|
|
|
+ TimeUpdate();
|
|
|
+ LineUpdate();
|
|
|
+ Spurhalten();
|
|
|
+ }
|
|
|
+ //}
|
|
|
|
|
|
//Schritt 3: Spurwechsel rechts
|
|
|
- //rechts drehen
|
|
|
+ //rechts drehen
|
|
|
+ turnAngle = 0;
|
|
|
rotationAngle = 0;
|
|
|
GyroUpdate();
|
|
|
while(rotationAngle > -45)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- motors.setSpeeds(FULLSPEED, HALFSPEED);
|
|
|
+ LineUpdate();
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
|
|
|
+ if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) break;
|
|
|
}
|
|
|
//geradeaus über Mittellinie fahren
|
|
|
LineUpdate();
|
|
|
while(lineValue[0] < MARKLINE0)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
- motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ //if(lineValue[0] > MARKLINE0) break;
|
|
|
}
|
|
|
//links drehen
|
|
|
GyroUpdate();
|
|
|
- while(rotationAngle < 0)
|
|
|
+ while(rotationAngle < -10)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- motors.setSpeeds(HALFSPEED, FULLSPEED);
|
|
|
+ LineUpdate();
|
|
|
+ if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(0, FULLSPEEDRIGHT);
|
|
|
+ else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
}
|
|
|
//geradeaus fahren
|
|
|
- Serial1.println("Umfahren beendet");
|
|
|
- motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
- lastUpdate = millis();
|
|
|
+ //Serial1.println("Umfahren beendet");
|
|
|
+ Serial1.println(0);
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
}
|
|
|
|
|
|
//Funktion Abbiegen
|
|
|
void Abbiegen()
|
|
|
{
|
|
|
- dir = 'A';
|
|
|
ledYellow(1);
|
|
|
- Serial1.println("Abbiegen");
|
|
|
+ //Serial1.println("Abbiegen");
|
|
|
+ Serial1.println(1);
|
|
|
|
|
|
//Markierung analysieren
|
|
|
+ LineUpdate();
|
|
|
bool leftCode; //links => 1
|
|
|
bool rightCode; //rechts => 2
|
|
|
- if(lineValue[0] > SIGN) leftCode = true;
|
|
|
+ if(lineValue[0] > SIGN0) leftCode = true;
|
|
|
else leftCode = false;
|
|
|
- if(lineValue[2] > SIGN) rightCode = true;
|
|
|
+ if(lineValue[2] > SIGN2) 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 = random(0, 2); //0, 1
|
|
|
- else if(leftCode == false && rightCode == true)
|
|
|
+ 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
|
|
|
+ // }
|
|
|
+
|
|
|
+ //links Abbiegen (von der Verbindungsstrecke)
|
|
|
+ if(randy == 1 && rightCode == true)
|
|
|
{
|
|
|
- randy = random(3); //0, (1), 2
|
|
|
- while(randy == 1) randy = random(3); //!1 => 0, 2
|
|
|
+ //Serial1.println("links Abbiegen von der Verbindungsstrecke");
|
|
|
+ //geradeaus zur Mittellinie fahren
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10);
|
|
|
+ lastUpdate = millis();
|
|
|
+ TimeUpdate();
|
|
|
+ while(eventTime < 300)
|
|
|
+ {
|
|
|
+ TimeUpdate();
|
|
|
+ //Serial1.println(eventTime);
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT+10);
|
|
|
+ }
|
|
|
+ LineUpdate();
|
|
|
+ //Serial1.println("Linie suchen");
|
|
|
+ while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
|
|
|
+ {
|
|
|
+ LineUpdate();
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ }
|
|
|
+ //links drehen
|
|
|
+ turnAngle = 0;
|
|
|
+ rotationAngle = 0;
|
|
|
+ GyroUpdate();
|
|
|
+ //Serial1.println("Drehen");
|
|
|
+ while(rotationAngle < 90)
|
|
|
+ {
|
|
|
+ GyroUpdate();
|
|
|
+ LineUpdate();
|
|
|
+ if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(0, FULLSPEEDRIGHT);
|
|
|
+ else if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ else motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ }
|
|
|
+ //geradeaus fahren
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
}
|
|
|
|
|
|
- //links Abbiegen
|
|
|
- if(randy == 1 && leftCode == true)
|
|
|
+ //links Abbiegen (vom Rundkurs)
|
|
|
+ else if(randy == 1 && leftCode == true)
|
|
|
{
|
|
|
- //zur Kreuzungsmitte fahren
|
|
|
- driveRotation[0] = 0;
|
|
|
+ //Serial1.println("links Abbiegen vom Rundkurs");
|
|
|
+ //links drehen
|
|
|
+ turnAngle = 0;
|
|
|
+ rotationAngle = 0;
|
|
|
driveRotation[1] = 0;
|
|
|
- while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
|
|
|
+ GyroUpdate();
|
|
|
+ while(rotationAngle < 40)
|
|
|
{
|
|
|
+ GyroUpdate();
|
|
|
EncoderUpdate();
|
|
|
- motors.setSpeeds(HALFSPEED, HALFSPEED);
|
|
|
+ motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ if(driveRotation[1] > 1) break;
|
|
|
}
|
|
|
- //links drehen
|
|
|
- rotationAngle = 0;
|
|
|
+ driveRotation[1] = 0;
|
|
|
GyroUpdate();
|
|
|
- while(rotationAngle != 90)
|
|
|
+ while(rotationAngle < 85)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN) motors.setSpeeds(HALFSPEED, HALFSPEED);
|
|
|
- else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN) motors.setSpeeds(-SLOWSPEED, -SLOWSPEED);
|
|
|
- else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN) motors.setSpeeds(0, HALFSPEED);
|
|
|
- else motors.setSpeeds(SLOWSPEED, HALFSPEED);
|
|
|
+ EncoderUpdate();
|
|
|
+ motors.setSpeeds(SLOWSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ if(driveRotation[1] > 1) break;
|
|
|
+ }
|
|
|
+ //geradeaus fahren
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ lastUpdate = millis();
|
|
|
+ TimeUpdate();
|
|
|
+ while(eventTime < 1000)
|
|
|
+ {
|
|
|
+ TimeUpdate();
|
|
|
+ LineUpdate();
|
|
|
+ if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) break;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
@@ -443,74 +546,95 @@ void Abbiegen()
|
|
|
else if(randy == 2 && rightCode == true)
|
|
|
{
|
|
|
//rechts drehen
|
|
|
+ turnAngle = 0;
|
|
|
rotationAngle = 0;
|
|
|
+ driveRotation[0] = 0;
|
|
|
GyroUpdate();
|
|
|
- while(rotationAngle != -90)
|
|
|
+ while(rotationAngle > -40)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN) motors.setSpeeds(HALFSPEED, 0);
|
|
|
- else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN) motors.setSpeeds(-SLOWSPEED, -SLOWSPEED);
|
|
|
- else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN) motors.setSpeeds(HALFSPEED, HALFSPEED);
|
|
|
- else motors.setSpeeds(HALFSPEED, SLOWSPEED);
|
|
|
+ EncoderUpdate();
|
|
|
+ LineUpdate();
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
|
|
|
+ if(driveRotation[0] > 1 || (lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0)) break;
|
|
|
+ }
|
|
|
+ GyroUpdate();
|
|
|
+ lastUpdate = millis();
|
|
|
+ while(rotationAngle > -85)
|
|
|
+ {
|
|
|
+ TimeUpdate();
|
|
|
+ GyroUpdate();
|
|
|
+ LineUpdate();
|
|
|
+ if(eventTime > 3000) break;
|
|
|
+ if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0) motors.setSpeeds(FULLSPEEDLEFT, 0);
|
|
|
+ //else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1) motors.setSpeeds(-SLOWSPEEDLEFT, -SLOWSPEEDRIGHT);
|
|
|
+ else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2) motors.setSpeeds(FULLSPEEDLEFT, HALFSPEEDRIGHT);
|
|
|
+ else motors.setSpeeds(FULLSPEEDLEFT, SLOWSPEEDRIGHT);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
//nicht Abbiegen, geradeaus fahren
|
|
|
- else motors.setSpeeds(HALFSPEED, HALFSPEED);
|
|
|
- delay(500);
|
|
|
-
|
|
|
- //geradeaus fahren
|
|
|
- Serial1.println("Abbiegen beendet");
|
|
|
- motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
- lastUpdate = millis();
|
|
|
+ else
|
|
|
+ {
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT, FULLSPEEDRIGHT);
|
|
|
+ lastUpdate = millis();
|
|
|
+ TimeUpdate();
|
|
|
+ while(eventTime < 1000)
|
|
|
+ {
|
|
|
+ TimeUpdate();
|
|
|
+ LineUpdate();
|
|
|
+ Spurhalten();
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //Serial1.println("Abbiegen beendet");
|
|
|
+ Serial1.println(0);
|
|
|
}
|
|
|
|
|
|
//Funktion Spurhalten
|
|
|
void Spurhalten()
|
|
|
{
|
|
|
//linke Linie erreicht, nach rechts fahren
|
|
|
- if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN)
|
|
|
+ if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN0)
|
|
|
{
|
|
|
- dir = 'R';
|
|
|
ledYellow(1);
|
|
|
//Serial1.println("Spur nach rechts korrigieren");
|
|
|
- motors.setSpeeds(FULLSPEED/eventSpeed, SLOWSPEED/eventSpeed);
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, SLOWSPEEDRIGHT/eventSpeed);
|
|
|
delay(100);
|
|
|
}
|
|
|
|
|
|
//rechte Linie erreicht, nach links fahren
|
|
|
- else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN)
|
|
|
+ else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN2)
|
|
|
{
|
|
|
- dir = 'L';
|
|
|
ledYellow(1);
|
|
|
//Serial1.println("Spur nach links korrigieren");
|
|
|
- motors.setSpeeds(SLOWSPEED/eventSpeed, FULLSPEED/eventSpeed);
|
|
|
+ motors.setSpeeds(SLOWSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);
|
|
|
delay(100);
|
|
|
}
|
|
|
|
|
|
//Linie überfahren, zurücksetzen
|
|
|
- else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN)
|
|
|
+ else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN1)
|
|
|
{
|
|
|
-
|
|
|
- dir = 'B';
|
|
|
ledRed(1);
|
|
|
- Serial1.println("Spurfinden");
|
|
|
- uint16_t startTime = millis();
|
|
|
+ //Serial1.println("Spurfinden");
|
|
|
+ Serial1.println(1);
|
|
|
+ lastUpdate = millis();
|
|
|
while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
|
|
|
{
|
|
|
- motors.setSpeeds(-HALFSPEED/eventSpeed, -HALFSPEED/eventSpeed);
|
|
|
- uint16_t backTime = millis();
|
|
|
- if((backTime - startTime) > 3000) return;
|
|
|
+ TimeUpdate();
|
|
|
+ LineUpdate();
|
|
|
+ motors.setSpeeds(-FULLSPEEDLEFT/eventSpeed, -FULLSPEEDRIGHT/eventSpeed);
|
|
|
+ if(eventTime > 3000) break;
|
|
|
}
|
|
|
delay(100);
|
|
|
- Serial1.println("Spur gefunden");
|
|
|
+ //Serial1.println("Spur gefunden");
|
|
|
+ Serial1.println(0);
|
|
|
}
|
|
|
|
|
|
//normale Fahrt
|
|
|
else
|
|
|
{
|
|
|
- dir = 'F';
|
|
|
ledGreen(1);
|
|
|
+ motors.setSpeeds(FULLSPEEDLEFT/eventSpeed, FULLSPEEDRIGHT/eventSpeed);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
@@ -532,8 +656,7 @@ void SerielleAusgabe()
|
|
|
|
|
|
void loop()
|
|
|
{
|
|
|
- LoopTiming(); //Zykluszeit beträgt max. 30ms im Idle
|
|
|
-
|
|
|
+ //LoopTiming(); //Zykluszeit beträgt max. 30ms im Idle
|
|
|
ledGreen(0);
|
|
|
ledYellow(0);
|
|
|
ledRed(0);
|
|
|
@@ -547,27 +670,27 @@ void loop()
|
|
|
TimeUpdate();
|
|
|
|
|
|
//Funktionsauswahl
|
|
|
- btData = Serial1.readString();
|
|
|
+ //btData = Serial1.readString();
|
|
|
+ if(Serial1.available() > 0) btData = Serial1.read();
|
|
|
+ if(btData == '1') alarm = true;
|
|
|
+ else if(btData == '0') alarm = false;
|
|
|
//verfügbare Funktionen bei laufenden Manövern
|
|
|
- if(btData == "Abbiegen" || btData == "Hindernisumfahren"
|
|
|
- || btData == "Kontaktvermeiden"|| btData == "Spurfinden")
|
|
|
+ //if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kontaktvermeiden"|| btData == "Spurfinden")
|
|
|
+ if(alarm == true)
|
|
|
{
|
|
|
//Serial1.println("Verstanden");
|
|
|
eventSpeed = 2;
|
|
|
- if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 ||
|
|
|
- lineValue[0] > SIGN || lineValue[2] > SIGN) motors.setSpeeds(0, 0);
|
|
|
+ if(moveRate > 1000 || proxValue[1] == 6 || proxValue[2] == 6 || lineValue[0] > SIGN0 || lineValue[2] > SIGN2) motors.setSpeeds(0, 0);
|
|
|
else Spurhalten();
|
|
|
}
|
|
|
//verfügbare Funktionen im Normalfall
|
|
|
else
|
|
|
{
|
|
|
eventSpeed = 1;
|
|
|
- if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();
|
|
|
- if(proxValue[1] == 6 || proxValue[2] == 6) motors.setSpeeds(0, 0);
|
|
|
- else if(proxValue[1] == 5 || proxValue[2] == 5) Hindernisumfahren();
|
|
|
- else if(lineValue[0] > SIGN || lineValue[2] > SIGN) Abbiegen();
|
|
|
+ //if(abs(moveRate) > 1500 && eventTime > 1000) Kontaktvermeiden();
|
|
|
+ if(proxValue[2] == 6) Hindernisumfahren();
|
|
|
+ else if(lineValue[0] > SIGN0 && lineValue[0] < 1000 || lineValue[2] > SIGN2 && lineValue[2] < 1000) Abbiegen();
|
|
|
else Spurhalten();
|
|
|
}
|
|
|
-
|
|
|
- LoopTiming();
|
|
|
+ //LoopTiming();
|
|
|
}
|
fstange