|
|
@@ -1,5 +1,5 @@
|
|
|
//Verfassser: Felix Stange, 4056379, MET2016
|
|
|
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 15.02.2018
|
|
|
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 01.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
|
|
|
@@ -15,19 +15,19 @@
|
|
|
|
|
|
Zumo32U4ProximitySensors proxSensors; //Abstandssensoren
|
|
|
Zumo32U4LineSensors lineSensors; //Liniensensoren
|
|
|
-Zumo32U4Motors motors;
|
|
|
-Zumo32U4ButtonA buttonA;
|
|
|
+Zumo32U4Motors motors; //Motoren
|
|
|
+Zumo32U4ButtonA buttonA; //Taste A
|
|
|
Zumo32U4Encoders encoders; //Motorencoder
|
|
|
LSM303 compass; //Beschleunigungssensor x-Achse
|
|
|
L3G gyro; //Drehbewegungssensor z-Achse
|
|
|
|
|
|
-int16_t lineValue[3]; //von links (0) nach rechts (2)
|
|
|
-uint16_t lineOffset[3];
|
|
|
+int16_t lineValue[3]; //Liniensensoren
|
|
|
+uint16_t lineOffset[3]; //von links (0) nach rechts (2)
|
|
|
|
|
|
-uint8_t proxValue[4]; //von links (0) nach rechts (3)
|
|
|
+uint8_t proxValue[4]; //Abstandssensoren v.l.(0)n.r.(3)
|
|
|
|
|
|
-int16_t encoderCounts[2]; //von links (0) nach rechts (1)
|
|
|
-int16_t driveRotation[2];
|
|
|
+int16_t encoderCounts[2]; //Anzahl der Umdrehungen
|
|
|
+int16_t driveRotation[2]; //von links (0) nach rechts (1)
|
|
|
|
|
|
int32_t rotationAngle = 0; //Drehwinkel
|
|
|
int32_t turnAngle = 0;
|
|
|
@@ -43,10 +43,10 @@ uint16_t compassLastUpdate;
|
|
|
|
|
|
uint16_t LastUpdate = 0; //Systemzeit
|
|
|
uint16_t CycleTime = 0; //Zykluszeit
|
|
|
-int maxSpeed = 200; //Maximale Geschwindigkeit (möglich 400)
|
|
|
+int maxSpeed = 200; //Maximale Geschwindigkeit (max. 400)
|
|
|
char dir; //Fahrtrichtung, Ereignis
|
|
|
char report[200]; //Ausgabe
|
|
|
-String btData;
|
|
|
+String btData; //Gelesene Daten aus Bluetooth
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
|
|
|
|
@@ -95,7 +95,7 @@ void GyroSetup()
|
|
|
int32_t total = 0;
|
|
|
for (uint16_t i = 0; i < 1024; i++)
|
|
|
{
|
|
|
- while(!gyro.readReg(L3G::STATUS_REG) & 0x08);
|
|
|
+ while(!gyro.readReg(L3G::STATUS_REG) & 0x08); //Fehlerabfrage
|
|
|
gyro.read();
|
|
|
total += gyro.g.z;
|
|
|
}
|
|
|
@@ -218,20 +218,23 @@ void EncoderUpdate()
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
|
|
|
|
- //Funktion Kollisionserkennung
|
|
|
-void Kollisionserkennung()
|
|
|
+//Funktion Kontaktvermeiden
|
|
|
+void Kontaktvermeiden()
|
|
|
{
|
|
|
dir = 'X';
|
|
|
- Serial1.println("Kollision");
|
|
|
+ Serial1.println("Kontaktvermeiden");
|
|
|
ledRed(1);
|
|
|
|
|
|
motors.setSpeeds(0, 0);
|
|
|
delay(500);
|
|
|
- while(lineValue[0] < 300 && lineValue[2] < 300)
|
|
|
+ while(proxValue[1] == 0 || proxValue[2] == 0)
|
|
|
{
|
|
|
+ ProxUpdate();
|
|
|
motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
+ if(lineValue[0] > 300 || lineValue[2] > 300) return;
|
|
|
}
|
|
|
delay(500);
|
|
|
+ Serial1.println("Vermeiden beendet");
|
|
|
}
|
|
|
|
|
|
//Funktion Hindernisumfahrung
|
|
|
@@ -268,21 +271,17 @@ void Hindernisumfahren()
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
|
|
|
//Gegenverkehr beachten
|
|
|
- proxSensors.read();
|
|
|
- proxValue[1] = proxSensors.countsFrontWithLeftLeds();
|
|
|
- proxValue[2] = proxSensors.countsFrontWithRightLeds();
|
|
|
+ ProxUpdate();
|
|
|
if(proxValue[1] < 5 || proxValue[2] < 5)
|
|
|
{
|
|
|
//Schritt 2: Hindernis passieren
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
delay(1000);
|
|
|
- proxSensors.read();
|
|
|
- proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
|
+ ProxUpdate();
|
|
|
while(proxValue[3] > 4)
|
|
|
{
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
- proxSensors.read();
|
|
|
- proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
|
+ ProxUpdate();
|
|
|
Spurhalten();
|
|
|
}
|
|
|
}
|
|
|
@@ -323,7 +322,7 @@ void Abbiegen()
|
|
|
ledYellow(1);
|
|
|
Serial1.println("Abbiegen");
|
|
|
|
|
|
- //Kodierung analysieren
|
|
|
+ //Markierung analysieren
|
|
|
bool leftCode; //links => 1
|
|
|
bool rightCode; //rechts => 2
|
|
|
if(lineValue[0] > 1000) leftCode = true;
|
|
|
@@ -420,13 +419,14 @@ void Spurhalten()
|
|
|
{
|
|
|
dir = 'B';
|
|
|
ledRed(1);
|
|
|
- Serial1.println("Spur verlassen");
|
|
|
+ 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
|
|
|
{
|
|
|
motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
}
|
|
|
delay(500);
|
|
|
+ Serial1.println("Spur gefunden");
|
|
|
}
|
|
|
|
|
|
//normale Fahrt
|
|
|
@@ -469,7 +469,8 @@ void loop()
|
|
|
//Funktionsauswahl
|
|
|
btData = Serial1.readString();
|
|
|
//verfügbare Funktionen bei laufenden Manövern
|
|
|
- if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kollision")
|
|
|
+ if(btData == "Abbiegen" || btData == "Hindernisumfahren"
|
|
|
+ || btData == "Kontaktvermeiden"|| btData == "Spurfinden")
|
|
|
{
|
|
|
maxSpeed = 100;
|
|
|
if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || lineValue[0] > 1000 ||
|
|
|
@@ -479,7 +480,7 @@ void loop()
|
|
|
//verfügbare Funktionen im Normalfall
|
|
|
else
|
|
|
{
|
|
|
- if(moveRate > 1000) Kollisionserkennung();
|
|
|
+ if(moveRate > 1000) Kontaktvermeiden();
|
|
|
else 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();
|
|
|
@@ -488,4 +489,4 @@ void loop()
|
|
|
|
|
|
//Ausgabe über Bluetoothverbindung
|
|
|
SerialOutput();
|
|
|
-}
|
|
|
+}
|
fstange