|
|
@@ -1,17 +1,17 @@
|
|
|
//Verfassser: Felix Stange, 4056379, MET2016
|
|
|
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 01.03.2018
|
|
|
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 26.03.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
|
|
|
-//dennoch zu einer Kollision, wird diese durch die Beschleunigungssensoren (LSM303) erkannt.
|
|
|
+//dennoch zu einer Kollision, wird diese durch den Beschleunigungssensor (LSM303) erkannt.
|
|
|
//Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G)
|
|
|
//genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden.
|
|
|
//Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die
|
|
|
//Kommunikation erfolgt seriell ('SERIAL' für USB, 'SERIAL1' für Bluetooth).
|
|
|
//Das LCD kann bei Bluetoothnutzung nicht verwendet werden.
|
|
|
|
|
|
-#include <Wire.h>
|
|
|
-#include <Zumo32U4.h>
|
|
|
+#include <Wire.h>
|
|
|
+#include <Zumo32U4.h>
|
|
|
|
|
|
Zumo32U4ProximitySensors proxSensors; //Abstandssensoren
|
|
|
Zumo32U4LineSensors lineSensors; //Liniensensoren
|
|
|
@@ -21,10 +21,19 @@ Zumo32U4Encoders encoders; //Motorencoder
|
|
|
LSM303 compass; //Beschleunigungssensor x-Achse
|
|
|
L3G gyro; //Drehbewegungssensor z-Achse
|
|
|
|
|
|
+#define MARKLINE0 200
|
|
|
+#define MARKLINE1 100
|
|
|
+#define MARKLINE2 200
|
|
|
+#define SIGN 1000
|
|
|
+#define MAXSPEED 400
|
|
|
+#define FULLSPEED 100
|
|
|
+#define HALFSPEED 50
|
|
|
+#define SLOWSPEED 25
|
|
|
+
|
|
|
int16_t lineValue[3]; //Liniensensoren
|
|
|
uint16_t lineOffset[3]; //von links (0) nach rechts (2)
|
|
|
|
|
|
-uint8_t proxValue[4]; //Abstandssensoren v.l.(0)n.r.(3)
|
|
|
+uint8_t proxValue[4]; //Abstandssensoren v.l. (0) n.r. (3)
|
|
|
|
|
|
int16_t encoderCounts[2]; //Anzahl der Umdrehungen
|
|
|
int16_t driveRotation[2]; //von links (0) nach rechts (1)
|
|
|
@@ -35,17 +44,17 @@ int16_t turnRate;
|
|
|
int16_t gyroOffset;
|
|
|
uint16_t gyroLastUpdate;
|
|
|
|
|
|
-int32_t moveDisplay = 0; //Beschleunigung
|
|
|
-int32_t moveDistance = 0;
|
|
|
+//int32_t moveDisplay = 0; //Beschleunigung
|
|
|
+//int32_t moveDistance = 0;
|
|
|
int16_t moveRate;
|
|
|
int16_t compassOffset;
|
|
|
-uint16_t compassLastUpdate;
|
|
|
+//uint16_t compassLastUpdate;
|
|
|
|
|
|
-uint16_t LastUpdate = 0; //Systemzeit
|
|
|
-uint16_t CycleTime = 0; //Zykluszeit
|
|
|
-int maxSpeed = 200; //Maximale Geschwindigkeit (max. 400)
|
|
|
-char dir; //Fahrtrichtung, Ereignis
|
|
|
+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
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
|
@@ -60,8 +69,8 @@ void LineSetup()
|
|
|
uint32_t total[3] = {0, 0, 0};
|
|
|
for(uint8_t i = 0; i < 120; i++)
|
|
|
{
|
|
|
- if (i > 30 && i <= 90) motors.setSpeeds(maxSpeed, -maxSpeed);
|
|
|
- else motors.setSpeeds(-maxSpeed, maxSpeed);
|
|
|
+ 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];
|
|
|
@@ -86,7 +95,16 @@ void GyroSetup()
|
|
|
{
|
|
|
ledYellow(1);
|
|
|
gyro.init();
|
|
|
-
|
|
|
+ if (!gyro.init())
|
|
|
+ {
|
|
|
+ //Fehler beim Initialisieren des Drehbewegungssensors
|
|
|
+ ledRed(1);
|
|
|
+ while(1)
|
|
|
+ {
|
|
|
+ Serial1.println(F("Fehler Drehbewegungssensors"));
|
|
|
+ delay(100);
|
|
|
+ }
|
|
|
+ }
|
|
|
gyro.writeReg(L3G::CTRL1, 0b11111111); //Ausgaberate 800Hz, Tiefpassfilter 100Hz
|
|
|
gyro.writeReg(L3G::CTRL4, 0b00100000); //2000dps Auflösung
|
|
|
gyro.writeReg(L3G::CTRL5, 0b00000000); //Hochpassfilter ausgeschaltet
|
|
|
@@ -110,6 +128,16 @@ void CompassSetup()
|
|
|
{
|
|
|
ledYellow(1);
|
|
|
compass.init();
|
|
|
+ if (!compass.init())
|
|
|
+ {
|
|
|
+ //Fehler beim Initialisieren des Beschleunigungssensors
|
|
|
+ ledRed(1);
|
|
|
+ while(1)
|
|
|
+ {
|
|
|
+ Serial.println(F("Fehler Beschleunigungssensors"));
|
|
|
+ delay(100);
|
|
|
+ }
|
|
|
+ }
|
|
|
compass.enableDefault();
|
|
|
|
|
|
//Kalibrierung
|
|
|
@@ -121,7 +149,7 @@ void CompassSetup()
|
|
|
}
|
|
|
compassOffset = total / 1024;
|
|
|
|
|
|
- compassLastUpdate = micros();
|
|
|
+ //compassLastUpdate = micros();
|
|
|
ledYellow(0);
|
|
|
}
|
|
|
|
|
|
@@ -130,14 +158,14 @@ void CompassSetup()
|
|
|
void setup()
|
|
|
{
|
|
|
//Initialisierung der Bluetoothverbindung
|
|
|
- Serial1.begin(9600);
|
|
|
+ Serial1.begin(9600);
|
|
|
+ Serial1.setTimeout(10); //verkürzt Serial.readString auf 10 ms statt 1s
|
|
|
if(Serial1.available()) Serial1.println("Verbindung hergestellt");
|
|
|
|
|
|
//Initialisierung und Kalibrierung der Sensoren
|
|
|
Serial1.println("Sensorkalibrierung");
|
|
|
Wire.begin();
|
|
|
delay(500);
|
|
|
- LastUpdate = micros();
|
|
|
ProxSetup();
|
|
|
LineSetup();
|
|
|
GyroSetup();
|
|
|
@@ -149,6 +177,8 @@ void setup()
|
|
|
buttonA.waitForButton();
|
|
|
randomSeed(millis());
|
|
|
delay(500);
|
|
|
+ motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
+ lastUpdate = millis();
|
|
|
}
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
|
@@ -156,9 +186,51 @@ void setup()
|
|
|
//Update Durchlaufzeit
|
|
|
void TimeUpdate()
|
|
|
{
|
|
|
- uint16_t m = micros();
|
|
|
- CycleTime = m - LastUpdate;
|
|
|
- LastUpdate = m;
|
|
|
+ uint16_t m = millis();
|
|
|
+ eventTime = m - lastUpdate;
|
|
|
+}
|
|
|
+
|
|
|
+void LoopTiming()
|
|
|
+{
|
|
|
+ const int AL = 100; // Arraylänge, NUR GERADE Zahlen verwenden!
|
|
|
+ static unsigned long LoopTime[AL];
|
|
|
+ static unsigned int Index=0, Messung=0, Min=0xFFFF, Max, Avg;
|
|
|
+
|
|
|
+ if (Messung % 2 == 0) // wenn Messung X gerade (0,2,4,6 usw.), entspricht immer Anfang der Loop
|
|
|
+ {
|
|
|
+ LoopTime[Index] = millis();
|
|
|
+ Messung++;
|
|
|
+ Index++;
|
|
|
+ return; // Funktion sofort beenden, spart etwas Zeit
|
|
|
+ }
|
|
|
+
|
|
|
+ if (Messung % 2 == 1) // wenn Messung X ungerade (1,3,5,7 usw.), entspricht immer Ende der Loop
|
|
|
+ {
|
|
|
+ LoopTime[Index] = millis();
|
|
|
+ LoopTime[Index-1] = LoopTime[Index] - LoopTime[Index-1]; // Loopdauer einen Index niedriger einspeichern wie aktuell
|
|
|
+ Messung++;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (Index >= AL) // Array voll, Daten auswerten
|
|
|
+ {
|
|
|
+ for (int i = 0; i<AL; i++)
|
|
|
+ {
|
|
|
+ Min = min(Min, LoopTime[i]);
|
|
|
+ Max = max(Max, LoopTime[i]);
|
|
|
+ Avg += LoopTime[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ Avg = Avg / AL;
|
|
|
+ Serial1.print(F("Minimal "));Serial1.print(Min);Serial1.println(" ms ");
|
|
|
+ Serial1.print(F("Durchschnitt "));Serial1.print(Avg);Serial1.println(" ms ");
|
|
|
+ Serial1.print(F("Maximal "));Serial1.print(Max);Serial1.println(" ms ");
|
|
|
+ SerielleAusgabe();
|
|
|
+ Min = 0xFFFF;
|
|
|
+ Max = 0;
|
|
|
+ Avg = 0;
|
|
|
+ Messung = 0;
|
|
|
+ Index = 0;
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
//Update Abstandssensoren
|
|
|
@@ -171,7 +243,7 @@ void ProxUpdate()
|
|
|
proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
|
}
|
|
|
|
|
|
-//Updaten Liniensensoren
|
|
|
+//Update Liniensensoren
|
|
|
void LineUpdate()
|
|
|
{
|
|
|
uint16_t lineRaw[3];
|
|
|
@@ -184,8 +256,8 @@ void LineUpdate()
|
|
|
//Update Drehbewegungssensor
|
|
|
void GyroUpdate()
|
|
|
{
|
|
|
- gyro.read();
|
|
|
- turnRate = gyro.g.z - gyroOffset;
|
|
|
+ gyro.read(); //Rohwert 10285 entspricht 90° bzw.
|
|
|
+ turnRate = gyro.g.z - gyroOffset; //8,75mdps/LSB
|
|
|
uint16_t m = micros();
|
|
|
uint16_t dt = m - gyroLastUpdate;
|
|
|
gyroLastUpdate = m;
|
|
|
@@ -194,17 +266,17 @@ void GyroUpdate()
|
|
|
rotationAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
|
|
|
}
|
|
|
|
|
|
-// Update Beschleunigungssensor
|
|
|
+//Update Beschleunigungssensor
|
|
|
void CompassUpdate()
|
|
|
{
|
|
|
- compass.read();
|
|
|
- moveRate = compass.a.x - compassOffset;
|
|
|
- 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 * 1000 / 9,81;
|
|
|
+ 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;
|
|
|
}
|
|
|
|
|
|
//Update Encoder
|
|
|
@@ -230,11 +302,14 @@ void Kontaktvermeiden()
|
|
|
while(proxValue[1] == 0 || proxValue[2] == 0)
|
|
|
{
|
|
|
ProxUpdate();
|
|
|
- motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
- if(lineValue[0] > 300 || lineValue[2] > 300) return;
|
|
|
+ motors.setSpeeds(-HALFSPEED, -HALFSPEED);
|
|
|
+ if(lineValue[0] > MARKLINE0 || lineValue[2] > MARKLINE2) return;
|
|
|
}
|
|
|
delay(500);
|
|
|
+
|
|
|
Serial1.println("Vermeiden beendet");
|
|
|
+ motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
+ lastUpdate = millis();
|
|
|
}
|
|
|
|
|
|
//Funktion Hindernisumfahrung
|
|
|
@@ -251,36 +326,36 @@ void Hindernisumfahren()
|
|
|
while(rotationAngle < 45)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
+ motors.setSpeeds(HALFSPEED, FULLSPEED);
|
|
|
}
|
|
|
//geradeaus über Mittellinie fahren
|
|
|
LineUpdate();
|
|
|
- while(lineValue[2] < 300)
|
|
|
+ while(lineValue[2] < MARKLINE2)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
}
|
|
|
//rechts drehen
|
|
|
GyroUpdate();
|
|
|
while(rotationAngle > 0)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
+ motors.setSpeeds(FULLSPEED, HALFSPEED);
|
|
|
}
|
|
|
//geradeaus fahren
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
|
|
|
//Gegenverkehr beachten
|
|
|
ProxUpdate();
|
|
|
if(proxValue[1] < 5 || proxValue[2] < 5)
|
|
|
{
|
|
|
//Schritt 2: Hindernis passieren
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
delay(1000);
|
|
|
ProxUpdate();
|
|
|
while(proxValue[3] > 4)
|
|
|
{
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
ProxUpdate();
|
|
|
Spurhalten();
|
|
|
}
|
|
|
@@ -293,26 +368,26 @@ void Hindernisumfahren()
|
|
|
while(rotationAngle > -45)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
+ motors.setSpeeds(FULLSPEED, HALFSPEED);
|
|
|
}
|
|
|
//geradeaus über Mittellinie fahren
|
|
|
LineUpdate();
|
|
|
- while(lineValue[0] < 300)
|
|
|
+ while(lineValue[0] < MARKLINE0)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
}
|
|
|
//links drehen
|
|
|
GyroUpdate();
|
|
|
while(rotationAngle < 0)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
+ motors.setSpeeds(HALFSPEED, FULLSPEED);
|
|
|
}
|
|
|
//geradeaus fahren
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
-
|
|
|
- Serial1.println("Umfahren beendet");
|
|
|
+ Serial1.println("Umfahren beendet");
|
|
|
+ motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
+ lastUpdate = millis();
|
|
|
}
|
|
|
|
|
|
//Funktion Abbiegen
|
|
|
@@ -325,9 +400,9 @@ void Abbiegen()
|
|
|
//Markierung analysieren
|
|
|
bool leftCode; //links => 1
|
|
|
bool rightCode; //rechts => 2
|
|
|
- if(lineValue[0] > 1000) leftCode = true;
|
|
|
+ if(lineValue[0] > SIGN) leftCode = true;
|
|
|
else leftCode = false;
|
|
|
- if(lineValue[2] > 1000) rightCode = true;
|
|
|
+ if(lineValue[2] > SIGN) rightCode = true;
|
|
|
else rightCode = false;
|
|
|
|
|
|
//Zufallszahl erzeugen
|
|
|
@@ -346,10 +421,10 @@ void Abbiegen()
|
|
|
//zur Kreuzungsmitte fahren
|
|
|
driveRotation[0] = 0;
|
|
|
driveRotation[1] = 0;
|
|
|
- while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < 300 && lineValue[2] < 300)
|
|
|
+ while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2)
|
|
|
{
|
|
|
EncoderUpdate();
|
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
+ motors.setSpeeds(HALFSPEED, HALFSPEED);
|
|
|
}
|
|
|
//links drehen
|
|
|
rotationAngle = 0;
|
|
|
@@ -357,13 +432,11 @@ void Abbiegen()
|
|
|
while(rotationAngle != 90)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- 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);
|
|
|
+ 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);
|
|
|
}
|
|
|
- //geradeaus fahren
|
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
}
|
|
|
|
|
|
//rechts Abbiegen
|
|
|
@@ -375,57 +448,61 @@ void Abbiegen()
|
|
|
while(rotationAngle != -90)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- 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);
|
|
|
+ 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);
|
|
|
}
|
|
|
- //geradeaus fahren
|
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
}
|
|
|
|
|
|
//nicht Abbiegen, geradeaus fahren
|
|
|
- else motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
+ else motors.setSpeeds(HALFSPEED, HALFSPEED);
|
|
|
delay(500);
|
|
|
|
|
|
- Serial1.println("Abbiegen beendet");
|
|
|
+ //geradeaus fahren
|
|
|
+ Serial1.println("Abbiegen beendet");
|
|
|
+ motors.setSpeeds(FULLSPEED, FULLSPEED);
|
|
|
+ lastUpdate = millis();
|
|
|
}
|
|
|
|
|
|
//Funktion Spurhalten
|
|
|
void Spurhalten()
|
|
|
{
|
|
|
//linke Linie erreicht, nach rechts fahren
|
|
|
- if(lineValue[0] > 300 && lineValue[0] < 500)
|
|
|
+ if(lineValue[0] > MARKLINE0 && lineValue[0] < SIGN)
|
|
|
{
|
|
|
dir = 'R';
|
|
|
ledYellow(1);
|
|
|
- Serial1.println("Spur nach rechts korrigieren");
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
+ //Serial1.println("Spur nach rechts korrigieren");
|
|
|
+ motors.setSpeeds(FULLSPEED/eventSpeed, SLOWSPEED/eventSpeed);
|
|
|
delay(100);
|
|
|
}
|
|
|
|
|
|
//rechte Linie erreicht, nach links fahren
|
|
|
- else if(lineValue[2] > 300 && lineValue[2] < 500)
|
|
|
+ else if(lineValue[2] > MARKLINE2 && lineValue[2] < SIGN)
|
|
|
{
|
|
|
dir = 'L';
|
|
|
ledYellow(1);
|
|
|
- Serial1.println("Spur nach links korrigieren");
|
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
+ //Serial1.println("Spur nach links korrigieren");
|
|
|
+ motors.setSpeeds(SLOWSPEED/eventSpeed, FULLSPEED/eventSpeed);
|
|
|
delay(100);
|
|
|
}
|
|
|
|
|
|
//Linie überfahren, zurücksetzen
|
|
|
- else if(lineValue[1] > 300 && lineValue[1] < 500)
|
|
|
+ else if(lineValue[1] > MARKLINE1 && lineValue[1] < SIGN)
|
|
|
{
|
|
|
+
|
|
|
dir = 'B';
|
|
|
ledRed(1);
|
|
|
Serial1.println("Spurfinden");
|
|
|
- Serial1.println("1");
|
|
|
- while(lineValue[0] < 300 && lineValue[2] < 300) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
|
|
|
+ uint16_t startTime = millis();
|
|
|
+ while(lineValue[0] < MARKLINE0 && lineValue[2] < MARKLINE2) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
|
|
|
{
|
|
|
- motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
+ motors.setSpeeds(-HALFSPEED/eventSpeed, -HALFSPEED/eventSpeed);
|
|
|
+ uint16_t backTime = millis();
|
|
|
+ if((backTime - startTime) > 3000) return;
|
|
|
}
|
|
|
- delay(500);
|
|
|
+ delay(100);
|
|
|
Serial1.println("Spur gefunden");
|
|
|
}
|
|
|
|
|
|
@@ -434,19 +511,20 @@ void Spurhalten()
|
|
|
{
|
|
|
dir = 'F';
|
|
|
ledGreen(1);
|
|
|
- Serial1.println("Spur folgen");
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
//Funktion Serielle Ausgabe
|
|
|
-void SerialOutput()
|
|
|
+void SerielleAusgabe()
|
|
|
{
|
|
|
snprintf_P(report, sizeof(report),
|
|
|
- PSTR("Abstand: %d %d %d %d Linie: %d %d %d Weg: %d %d Winkel: %d Zeit: %d"),
|
|
|
+ PSTR("Abstand: %d %d %d %d Linie: %d %d %d"),
|
|
|
proxValue[0], proxValue[1], proxValue[2], proxValue[3],
|
|
|
- lineValue[0], lineValue[1], lineValue[2],
|
|
|
- driveRotation[0], driveRotation[1], rotationAngle, CycleTime);
|
|
|
+ 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);
|
|
|
}
|
|
|
|
|
|
@@ -454,17 +532,19 @@ void SerialOutput()
|
|
|
|
|
|
void loop()
|
|
|
{
|
|
|
+ LoopTiming(); //Zykluszeit beträgt max. 30ms im Idle
|
|
|
+
|
|
|
ledGreen(0);
|
|
|
ledYellow(0);
|
|
|
ledRed(0);
|
|
|
|
|
|
//Abfragen der Sensordaten
|
|
|
- TimeUpdate();
|
|
|
LineUpdate();
|
|
|
ProxUpdate();
|
|
|
+ EncoderUpdate();
|
|
|
GyroUpdate();
|
|
|
CompassUpdate();
|
|
|
- EncoderUpdate();
|
|
|
+ TimeUpdate();
|
|
|
|
|
|
//Funktionsauswahl
|
|
|
btData = Serial1.readString();
|
|
|
@@ -472,21 +552,22 @@ void loop()
|
|
|
if(btData == "Abbiegen" || btData == "Hindernisumfahren"
|
|
|
|| btData == "Kontaktvermeiden"|| btData == "Spurfinden")
|
|
|
{
|
|
|
- maxSpeed = 100;
|
|
|
- if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || lineValue[0] > 1000 ||
|
|
|
- lineValue[2] > 1000) motors.setSpeeds(0, 0);
|
|
|
+ //Serial1.println("Verstanden");
|
|
|
+ eventSpeed = 2;
|
|
|
+ if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 ||
|
|
|
+ lineValue[0] > SIGN || lineValue[2] > SIGN) motors.setSpeeds(0, 0);
|
|
|
else Spurhalten();
|
|
|
}
|
|
|
//verfügbare Funktionen im Normalfall
|
|
|
else
|
|
|
{
|
|
|
- if(moveRate > 1000) Kontaktvermeiden();
|
|
|
- else if(proxValue[1] == 6 || proxValue[2] == 6) motors.setSpeeds(0, 0);
|
|
|
+ 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] > 1000 || lineValue[2] > 1000) Abbiegen();
|
|
|
+ else if(lineValue[0] > SIGN || lineValue[2] > SIGN) Abbiegen();
|
|
|
else Spurhalten();
|
|
|
}
|
|
|
|
|
|
- //Ausgabe über Bluetoothverbindung
|
|
|
- SerialOutput();
|
|
|
+ LoopTiming();
|
|
|
}
|
fstange