|
@@ -1,5 +1,5 @@
|
|
|
//Verfassser: Felix Stange 4056379 MET2016
|
|
|
-//Datum: 19.07.2017 erstellt, 20.10.2017 zuletzt geändert
|
|
|
+//Datum: 19.07.2017 erstellt, 25.10.2017 zuletzt geändert
|
|
|
//Projekt: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe der
|
|
|
//Liniensensoren (3), ähnlich einer Spurhalteautomatik (dunkler Belag und helle Streifen).
|
|
|
//Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es
|
|
@@ -230,22 +230,31 @@ void ObstacleAvoidance()
|
|
|
Serial1.println("obstacle avoidance");
|
|
|
ledYellow(1);
|
|
|
|
|
|
- //Schritt 1: links bis über Mittellinie fahren
|
|
|
+ //Schritt 1: Spurwechsel links
|
|
|
+ //links drehen
|
|
|
rotationAngle = 0;
|
|
|
GyroUpdate();
|
|
|
- while(lineValue[2] > 1000)
|
|
|
+ while(rotationAngle < 45)
|
|
|
{
|
|
|
- LineUpdate();
|
|
|
+ GyroUpdate();
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
}
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
-
|
|
|
- //Schritt 2: rechts fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
|
|
|
- while(rotationAngle != 0)
|
|
|
+ //geradeaus über Mittellinie fahren
|
|
|
+ LineUpdate();
|
|
|
+ while(lineValue[2] > 1000)
|
|
|
+ {
|
|
|
+ LineUpdate();
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ }
|
|
|
+ //rechts drehen
|
|
|
+ GyroUpdate();
|
|
|
+ while(rotationAngle > 0)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
}
|
|
|
+ //geradeaus fahren
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
|
|
|
//Gegenverkehr beachten
|
|
|
proxSensors.read();
|
|
@@ -253,9 +262,9 @@ void ObstacleAvoidance()
|
|
|
proxValue[2] = proxSensors.countsFrontWithRightLeds();
|
|
|
if(proxValue[1] < 4 || proxValue[2] < 4)
|
|
|
{
|
|
|
- //Schritt 3: geradeaus fahren bis Hindernis passiert
|
|
|
+ //Schritt 2: Hindernis passieren
|
|
|
maxSpeed = 400;
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
delay(1000);
|
|
|
proxSensors.read();
|
|
|
proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
@@ -269,29 +278,36 @@ void ObstacleAvoidance()
|
|
|
maxSpeed = 200;
|
|
|
}
|
|
|
|
|
|
- //Schritt 4: rechts bis über Mittellinie fahren
|
|
|
+ //Schritt 3: Spurwechsel rechts
|
|
|
+ //rechts drehen
|
|
|
rotationAngle = 0;
|
|
|
GyroUpdate();
|
|
|
- while(lineValue[0] > 1000)
|
|
|
+ while(rotationAngle > -45)
|
|
|
{
|
|
|
- LineUpdate();
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
+ GyroUpdate();
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
}
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
-
|
|
|
- //Schritt 5: links fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
|
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
- while(rotationAngle != 0)
|
|
|
+ //geradeaus über Mittellinie fahren
|
|
|
+ LineUpdate();
|
|
|
+ while(lineValue[0] > 1000)
|
|
|
+ {
|
|
|
+ LineUpdate();
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ }
|
|
|
+ //links drehen
|
|
|
+ GyroUpdate();
|
|
|
+ while(rotationAngle < 0)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- TrackKeeper();
|
|
|
- }
|
|
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
+ }
|
|
|
+ //geradeaus fahren
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
}
|
|
|
|
|
|
//Funktion Spurhalten
|
|
|
void TrackKeeper()
|
|
|
{
|
|
|
-
|
|
|
//linke Linie erreicht, nach rechts fahren
|
|
|
if(lineValue[0] < 1000)
|
|
|
{
|
|
@@ -322,7 +338,7 @@ void TrackKeeper()
|
|
|
dir = 'B';
|
|
|
}
|
|
|
|
|
|
- //Normale Fahrt
|
|
|
+ //normale Fahrt
|
|
|
else
|
|
|
{
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
@@ -363,7 +379,6 @@ void Crossroad()
|
|
|
EncoderUpdate();
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
}
|
|
|
-
|
|
|
//links drehen
|
|
|
rotationAngle = 0;
|
|
|
GyroUpdate();
|
|
@@ -372,7 +387,6 @@ void Crossroad()
|
|
|
GyroUpdate();
|
|
|
motors.setSpeeds(0, maxSpeed/2);
|
|
|
}
|
|
|
-
|
|
|
//geradeaus fahren
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
}
|
|
@@ -388,7 +402,6 @@ void Crossroad()
|
|
|
GyroUpdate();
|
|
|
motors.setSpeeds(maxSpeed/2, 0);
|
|
|
}
|
|
|
-
|
|
|
//geradeaus fahren
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
}
|