|
@@ -42,13 +42,10 @@ int16_t moveRate;
|
|
|
int16_t compassOffset;
|
|
|
uint16_t compassLastUpdate;
|
|
|
|
|
|
-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
|
|
|
+char dir = "0"; //Fahrtrichtung, Ereignis
|
|
|
char report[200]; //Ausgabe
|
|
|
+char attention; //zeigt Überhol-/Abbiegevorgang an
|
|
|
|
|
|
/*-----------------------------------------------------------------*/
|
|
|
|
|
@@ -133,10 +130,10 @@ void setup()
|
|
|
{
|
|
|
//Initialisierung der Bluetoothverbindung
|
|
|
Serial1.begin(9600);
|
|
|
- if(Serial1.available()) Serial1.println("bluetooth available");
|
|
|
+ if(Serial1.available()) Serial1.println("Bluetooth verfügbar");
|
|
|
|
|
|
//Initialisierung und Kalibrierung der Sensoren
|
|
|
- Serial1.println("sensor calibration, dont touch");
|
|
|
+ Serial1.println("Sensorkalibrierung");
|
|
|
Wire.begin();
|
|
|
delay(500);
|
|
|
ProxSetup();
|
|
@@ -145,7 +142,7 @@ void setup()
|
|
|
CompassSetup();
|
|
|
|
|
|
//Zumo bereit zu starten
|
|
|
- Serial1.println("Zumo ready to start, press A");
|
|
|
+ Serial1.println("Zumo bereit, drücke A");
|
|
|
ledGreen(1);
|
|
|
buttonA.waitForButton();
|
|
|
randomSeed(millis());
|
|
@@ -203,7 +200,7 @@ void CompassUpdate()
|
|
|
void EncoderUpdate()
|
|
|
{
|
|
|
encoderCounts[0] = encoders.getCountsLeft();
|
|
|
- driveRotation[0] = encoderCounts[0] / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
|
|
+ driveRotation[0] = encoderCounts[0] / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
|
|
encoderCounts[1] = encoders.getCountsRight();
|
|
|
driveRotation[1] = encoderCounts[1] / 910;
|
|
|
}
|
|
@@ -213,21 +210,21 @@ void EncoderUpdate()
|
|
|
//Funktion Kollisionserkennung
|
|
|
void CollisionDetection()
|
|
|
{
|
|
|
- dir = 'X';
|
|
|
- Serial1.println("impact detected");
|
|
|
+ dir = "X";
|
|
|
+ Serial1.println("Aufprall erkannt");
|
|
|
ledRed(1);
|
|
|
|
|
|
motors.setSpeeds(0, 0);
|
|
|
delay(500);
|
|
|
motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
- delay(1000);
|
|
|
+ delay(500);
|
|
|
}
|
|
|
|
|
|
//Funktion Hindernisumfahrung
|
|
|
void ObstacleAvoidance()
|
|
|
{
|
|
|
- dir = 'U';
|
|
|
- Serial1.println("obstacle avoidance");
|
|
|
+ dir = "U";
|
|
|
+ Serial1.println("Überholen");
|
|
|
ledYellow(1);
|
|
|
|
|
|
//Schritt 1: Spurwechsel links
|
|
@@ -263,7 +260,6 @@ void ObstacleAvoidance()
|
|
|
if(proxValue[1] < 4 || proxValue[2] < 4)
|
|
|
{
|
|
|
//Schritt 2: Hindernis passieren
|
|
|
- maxSpeed = 400;
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
delay(1000);
|
|
|
proxSensors.read();
|
|
@@ -275,7 +271,6 @@ void ObstacleAvoidance()
|
|
|
proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
|
TrackKeeper();
|
|
|
}
|
|
|
- maxSpeed = 200;
|
|
|
}
|
|
|
|
|
|
//Schritt 3: Spurwechsel rechts
|
|
@@ -303,6 +298,8 @@ void ObstacleAvoidance()
|
|
|
}
|
|
|
//geradeaus fahren
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+
|
|
|
+ Serial1.println("Überholen beendet");
|
|
|
}
|
|
|
|
|
|
//Funktion Spurhalten
|
|
@@ -314,7 +311,7 @@ void TrackKeeper()
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
ledYellow(1);
|
|
|
delay(100);
|
|
|
- dir = 'R';
|
|
|
+ dir = "R";
|
|
|
}
|
|
|
|
|
|
//rechte Linie erreicht, nach links fahren
|
|
@@ -323,7 +320,7 @@ void TrackKeeper()
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
ledYellow(1);
|
|
|
delay(100);
|
|
|
- dir = 'L';
|
|
|
+ dir = "L";
|
|
|
}
|
|
|
|
|
|
//Linie überfahren, zurücksetzen
|
|
@@ -335,7 +332,7 @@ void TrackKeeper()
|
|
|
motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
}
|
|
|
delay(500);
|
|
|
- dir = 'B';
|
|
|
+ dir = "B";
|
|
|
}
|
|
|
|
|
|
//normale Fahrt
|
|
@@ -343,7 +340,7 @@ void TrackKeeper()
|
|
|
{
|
|
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
ledGreen(1);
|
|
|
- dir = 'F';
|
|
|
+ dir = "F";
|
|
|
}
|
|
|
}
|
|
|
|
|
@@ -351,15 +348,19 @@ void TrackKeeper()
|
|
|
void Crossroad()
|
|
|
{
|
|
|
ledYellow(1);
|
|
|
- dir = 'A';
|
|
|
+ dir = "A";
|
|
|
+ Serial1.println("Abbiegen");
|
|
|
|
|
|
//Kodierung analysieren
|
|
|
+ bool leftCode; //links => 1
|
|
|
+ bool rightCode; //rechts => 2
|
|
|
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
|
|
|
+ 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)
|
|
@@ -408,6 +409,8 @@ void Crossroad()
|
|
|
|
|
|
//nicht Abbiegen, geradeaus fahren
|
|
|
else motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
+
|
|
|
+ Serial1.println("Abbiegen beendet");
|
|
|
}
|
|
|
|
|
|
//Funktion Serielle Ausgabe
|
|
@@ -437,11 +440,22 @@ void loop()
|
|
|
EncoderUpdate();
|
|
|
|
|
|
//Funktionsauswahl
|
|
|
- if(moveRate > 1000) CollisionDetection();
|
|
|
- else if(proxValue[0] == 6 || proxValue[1] == 6) motors.setSpeeds(0, 0);
|
|
|
- else if(proxValue[0] > 4 || proxValue[1] > 4) ObstacleAvoidance();
|
|
|
- else if((lineValue[0] > 500 && lineValue[0] < 1500) || (lineValue[2] > 500 && lineValue[2] < 1500)) Crossroad();
|
|
|
- else TrackKeeper();
|
|
|
+ attention = Serial1.read();
|
|
|
+ if(attention == "Überholen" || "Abbiegen")
|
|
|
+ {
|
|
|
+ maxSpeed = 100;
|
|
|
+ if(moveRate > 1000 || proxValue[0] > 4 || proxValue[1] > 4 || lineValue[0] > 500 && lineValue[0] < 1500) ||
|
|
|
+ lineValue[2] > 500 && lineValue[2] < 1500) motors.setSpeeds(0, 0);
|
|
|
+ else TrackKeeper();
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(moveRate > 1000) CollisionDetection();
|
|
|
+ else if(proxValue[0] == 6 || proxValue[1] == 6) motors.setSpeeds(0, 0);
|
|
|
+ else if(proxValue[0] > 4 || proxValue[1] > 4) ObstacleAvoidance();
|
|
|
+ else if((lineValue[0] > 500 && lineValue[0] < 1500) || (lineValue[2] > 500 && lineValue[2] < 1500)) Crossroad();
|
|
|
+ else TrackKeeper();
|
|
|
+ }
|
|
|
|
|
|
//Ausgabe über Bluetoothverbindung
|
|
|
SerialOutput();
|