Update function obstacleAvoidance

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -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); 
   }