Update for functions ObstacleAvoidance and Crossroad

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -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