|
@@ -44,7 +44,9 @@ int16_t moveRate;
|
|
|
int16_t compassOffset;
|
|
|
uint16_t compassLastUpdate;
|
|
|
|
|
|
-uint8_t randy; //Zufallszahl für Richtung
|
|
|
+uint8_t randy; //Richtungsauswahl Abbiegen geradeaus => 0
|
|
|
+bool leftCode; // links => 1
|
|
|
+bool rightCode; // rechts => 2
|
|
|
|
|
|
int maxSpeed = 200; //Maximale Geschwindigkeit (möglich 400)
|
|
|
char dir = '0'; //Fahrtrichtung, Ereignis
|
|
@@ -237,6 +239,7 @@ void ObstacleAvoidance()
|
|
|
LineUpdate();
|
|
|
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)
|
|
@@ -267,15 +270,15 @@ void ObstacleAvoidance()
|
|
|
maxSpeed = 200;
|
|
|
}
|
|
|
|
|
|
- //Schritt 4: rechts bis über Mittellinie fahren
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
- ledYellow(1);
|
|
|
+ //Schritt 4: rechts bis über Mittellinie fahren
|
|
|
rotationAngle = 0;
|
|
|
GyroUpdate();
|
|
|
- while(lineValue[0] < 1000)
|
|
|
+ while(lineValue[0] > 1000)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
+ 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);
|
|
@@ -295,7 +298,7 @@ void TrackKeeper()
|
|
|
{
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
ledYellow(1);
|
|
|
- delay(200);
|
|
|
+ delay(100);
|
|
|
dir = 'R';
|
|
|
}
|
|
|
|
|
@@ -304,19 +307,19 @@ void TrackKeeper()
|
|
|
{
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
ledYellow(1);
|
|
|
- delay(200);
|
|
|
+ delay(100);
|
|
|
dir = 'L';
|
|
|
}
|
|
|
|
|
|
//Linie überfahren, zurücksetzen
|
|
|
else if(lineValue[1] < 1000)
|
|
|
{
|
|
|
- while(lineValue[0] > 1000)
|
|
|
+ ledRed(1);
|
|
|
+ while(lineValue[0] > 1000 && lineValue[2] > 1000)
|
|
|
{
|
|
|
motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
}
|
|
|
- ledRed(1);
|
|
|
- delay(1000);
|
|
|
+ delay(500);
|
|
|
dir = 'B';
|
|
|
}
|
|
|
|
|
@@ -325,7 +328,6 @@ void TrackKeeper()
|
|
|
{
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
ledGreen(1);
|
|
|
- delay(100);
|
|
|
dir = 'F';
|
|
|
}
|
|
|
}
|
|
@@ -336,14 +338,23 @@ void Crossroad()
|
|
|
ledYellow(1);
|
|
|
dir = 'A';
|
|
|
|
|
|
- //Zufallszahl erzeugen
|
|
|
- randy = random(3);
|
|
|
-
|
|
|
//Kodierung analysieren
|
|
|
+ if(lineValue[0] > 500 && lineValue[0] < 1500) leftCode = true;
|
|
|
+ else leftCode = false;
|
|
|
+ if(lineValue[2] > 500 && lineValue[2] < 1500) rightCode = true;
|
|
|
+ else rightCode = false;
|
|
|
|
|
|
+ //Zufallszahl erzeugen
|
|
|
+ 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)
|
|
|
+ {
|
|
|
+ randy = random(3); //0, (1), 2
|
|
|
+ while(randy == 1) randy = random(3); //!1 => 0, 2
|
|
|
+ }
|
|
|
|
|
|
//links Abbiegen
|
|
|
- if(randy == 1)
|
|
|
+ if(randy == 1 && leftCode == true)
|
|
|
{
|
|
|
//zur Kreuzungsmitte fahren
|
|
|
wayLeft = 0;
|
|
@@ -360,7 +371,7 @@ void Crossroad()
|
|
|
while(rotationAngle != 90)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- motors.setSpeeds(0, maxSpeed);
|
|
|
+ motors.setSpeeds(0, maxSpeed/2);
|
|
|
}
|
|
|
|
|
|
//geradeaus fahren
|
|
@@ -368,22 +379,23 @@ void Crossroad()
|
|
|
}
|
|
|
|
|
|
//rechts Abbiegen
|
|
|
- else if(randy == 2)
|
|
|
+ else if(randy == 2 && rightCode == true)
|
|
|
{
|
|
|
//rechts drehen
|
|
|
rotationAngle = 0;
|
|
|
GyroUpdate();
|
|
|
- while(rotationAngle != 90)
|
|
|
+ while(rotationAngle != -90)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
- motors.setSpeeds(maxSpeed, 0);
|
|
|
+ motors.setSpeeds(maxSpeed/2, 0);
|
|
|
}
|
|
|
|
|
|
//geradeaus fahren
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
}
|
|
|
|
|
|
- else motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ //nicht Abbiegen, geradeaus fahren
|
|
|
+ else motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
}
|
|
|
|
|
|
//Funktion Serielle Ausgabe
|
|
@@ -415,7 +427,7 @@ void loop()
|
|
|
//Funktionsauswahl
|
|
|
/* if(moveRate > 1000) CollisionDetection();
|
|
|
else if(proxValue[0] > 4 || proxValue[1] > 4) ObstacleAvoidance();
|
|
|
- else if(lineValue[0] < 1000 && lineValue[1] < 1000 && lineValue[2] < 1000) Crossroad();
|
|
|
+ else if((lineValue[0] > 500 && lineValue[0] < 1500) || (lineValue[2] > 500 && lineValue[2] < 1500) Crossroad();
|
|
|
else TrackKeeper(); */
|
|
|
|
|
|
//Ausgabe über Bluetoothverbindung
|