Added Function Crossroad

README.md

@@ -3,7 +3,7 @@
 
 Es werden „Zumo 32U4 Robot“ der Firma „Pololu“ bereitgestellt. Diese verfügen über einen Antrieb, sowie eine Vielzahl von Sensoren. Die Programmierung erfolgt mithilfe der Arduino IDE. Mit diesen Roboterfahrzeugen sollen folgende Aufgaben realisiert werden: 
 
-- Anpassen der Geschwindigkeit entsprechend der     Fahrsituation, z.B. langsames Heranfahren an Hindernisse 
+- Anpassen der Geschwindigkeit entsprechend der Fahrsituation, z.B. langsames Heranfahren an Hindernisse 
 - Vor dem Abbiegen Geschwindigkeit reduzieren
 - Automatischer gegenläufiger Fahrbetrieb auf einer mehrspurigen Straße mit angepasster Geschwindigkeit
 - Anfertigung einer geeigneten Fahrstrecke mit Abbiegemöglichkeit

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -24,13 +24,15 @@ uint16_t  lineOffset[3];
 
 uint8_t   proxValues[4];                //von links (0) nach rechts (3)
 
-uint32_t  turnAngle;                    //Drehwinkel in °
+uint32_t  turnAngle;                    //Drehwinkel
 int16_t   turnRate;
 int16_t   gyroOffset;
 uint16_t  gyroLastUpdate;
 
 int16_t   compassOffset;
 
+uint4_t   rand;                         //Zufallszahl für Richtung
+
 int       maxSpeed = 200;               //Maximale Geschwindigkeit (möglich 400)
 char      dir;                          //Fahrtrichtung, Ereignis
 char      report[120];                  //Ausgabe
@@ -115,7 +117,7 @@ void setup()
 
   //Kalibrierung der Sensoren
   Serial1.println("sensor calibration");                            
-  buttonA.waitForButton();  
+  buttonA.waitForButton();
   LineSensorSetup();                                                
   TurnSensorSetup();
   MoveSensorSetup();
@@ -201,8 +203,6 @@ void ObstacleAvoidance()
   {                                                                   
     TurnSensorUpdate();
   }  
-
-  ledYellow(0);
 }
 
 //Funktion Spurhalten
@@ -210,7 +210,7 @@ void TrackKeeper()
 {
 
   //linke Linie erreicht, nach rechts fahren
-  if(lineValues[0] < (lineOffset[0] - 200))                          
+  if(lineValues[0] > (lineOffset[0] + 200))                          
   {
     motors.setSpeeds(maxSpeed, 0);
     ledYellow(1);
@@ -219,7 +219,7 @@ void TrackKeeper()
   }
 
   //rechte Linie erreicht, nach links fahren
-  else if(lineValues[2] < (lineOffset[2] - 200))                     
+  else if(lineValues[2] > (lineOffset[2] + 200))                     
   {
     motors.setSpeeds(0, maxSpeed);  
     ledYellow(1); 
@@ -228,7 +228,7 @@ void TrackKeeper()
   }
 
   //Linie überfahren, zurücksetzen
-  else if(lineValues[1] < (lineOffset[1] - 100))                     
+  else if(lineValues[1] > (lineOffset[1] + 200))                     
   { 
     motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
     ledRed(1); 
@@ -261,7 +261,50 @@ void TurnSensorUpdate()
 //Funktion Abbiegen
 void Crossroad()
 {
+  ledYellow(1); 
+  dir = 'A';
+
+  //Zufallszahl erzeugen
+  rand = random(3);
+
+  //Kodierung analysieren
+
+
+  //links Abbiegen
+  if(rand == 1) 
+  {
+    //zur Kreuzungsmitte fahren
+    while((lineValues[0] < (lineOffset[0] + 200)) || (lineValues[1] < (lineOffset[1] + 200)) 
+    || (lineValues[2] < (lineOffset[2] + 200))) motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
+
+    //links drehen
+    turnAngle = 0;
+    TurnSensorUpdate();
+    while(turnAngle != 90)
+    {
+    motors.setSpeeds(0, maxSpeed);  
+    TurnSensorUpdate();
+    }
+
+    //geradeaus fahren
+    motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
+  }
+
+  //rechts Abbiegen
+  else if(rand == 2) 
+  {
+    //rechts drehen
+    turnAngle = 0;
+    TurnSensorUpdate();
+    while(turnAngle != 90)
+    {
+    motors.setSpeeds(maxSpeed, 0);  
+    TurnSensorUpdate();
+    }
 
+    //geradeaus fahren
+    motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
+  }  
 }
 
 //Funktion Serielle Ausgabe
@@ -294,7 +337,8 @@ void loop()
   //Funktionsauswahl
   if((compass.a.x - compassOffset) > 16000) CollisionDetection();         
   else if((proxValues[0] || proxValues[1]) > 4) ObstacleAvoidance();
-  else if (lineValue[0] && lineValue[1] && lineValue[2]) Crossroad();
+  else if((lineValues[0] > (lineOffset[0] + 200)) && (lineValues[1] > (lineOffset[1] + 200)) 
+    && (lineValues[2] > (lineOffset[2] + 200))) Crossroad();
   else TrackKeeper();
 
   //Ausgabe über Bluetoothverbindung