General improvements

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

 //Verfassser: Felix Stange 4056379 MET2016
-//Datum: 19.07.2017 erstellt, 11.10.2017 zuletzt geändert
+//Datum: 19.07.2017 erstellt, 18.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 
 LSM303                    compass;      //Beschleunigungssensor x-Achse
 L3G                       gyro;         //Drehbewegungssensor z-Achse
 
-uint16_t  lineValue[3];                //von links (0) nach rechts (2)
+uint16_t  lineValue[3];                 //von links (0) nach rechts (2)
 uint16_t  lineOffset[3]; 
 uint16_t  lineRaw[3];
 
-uint8_t   proxValue[4];                //von links (0) nach rechts (3)
+uint8_t   proxValue[4];                 //von links (0) nach rechts (3)
 
-int16_t   countsLeft;                   //Encoder rechts
+int16_t   countsLeft;                   //Encoder links
 int16_t   wayLeft;
-int16_t   countsRight;                  //Encoder links
+int16_t   countsRight;                  //Encoder rechts
 int16_t   wayRight;
 
 int32_t   rotationAngle = 0;            //Drehwinkel
   
   //Zumo bereit zu starten
   Serial1.println("Zumo ready to start, press A");
+  ledGreen(1);
   buttonA.waitForButton();
   delay(500);
 }
 }
 
 //Update Encoder
-
 void EncoderUpdate()
 {
   countsLeft = encoders.getCountsLeft();
-  wayLeft = countsRight / 910;                            //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
+  wayLeft = countsRight / 910;                                //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
   countsRight = encoders.getCountsRight();
   wayRight = countsRight / 910;
 }
   Serial1.println("obstacle avoidance");  
   ledYellow(1);
 
-  //Schritt 1: links bis über Mittellinie fahren
-  motors.setSpeeds(maxSpeed/2, maxSpeed);                             
+  //Schritt 1: links bis über Mittellinie fahren                            
   rotationAngle = 0;                                                      
   GyroUpdate();                                                 
   while(lineValue[2] > 1000)              
   {
     LineUpdate();
+    motors.setSpeeds(maxSpeed/2, maxSpeed); 
   }
 
-  //Schritt 2: rechts fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
-  motors.setSpeeds(maxSpeed, maxSpeed/2);                             
+  //Schritt 2: rechts fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren                            
   while(rotationAngle != 0)                                               
   {                                                                   
     GyroUpdate();
+    motors.setSpeeds(maxSpeed, maxSpeed/2); 
   }
 
   //Gegenverkehr beachten
   proxSensors.read();                                                 
   proxValue[1] = proxSensors.countsFrontWithLeftLeds();
   proxValue[2] = proxSensors.countsFrontWithRightLeds();  
-  if((proxValue[1] || proxValue[2]) < 4)
+  if(proxValue[1] < 4 || proxValue[2] < 4)
   {
     //Schritt 3: geradeaus fahren bis Hindernis passiert
     maxSpeed = 400;
     proxValue[3] = proxSensors.countsRightWithRightLeds();  
     while(proxValue[3] > 4)
     {
+      motors.setSpeeds(maxSpeed, maxSpeed);  
       proxSensors.read();
       proxValue[3] = proxSensors.countsRightWithRightLeds();  
       TrackKeeper();
   }
   
   //Schritt 4: rechts bis über Mittellinie fahren
-  motors.setSpeeds(maxSpeed/2, maxSpeed);                             
+  motors.setSpeeds(maxSpeed, maxSpeed/2);                             
   ledYellow(1);
   rotationAngle = 0;                                                      
   GyroUpdate();                                                 
-  while(lineValue[0] - 200)     
+  while(lineValue[0] < 1000)     
   {
     LineUpdate();
   }
   while(rotationAngle != 0)                                               
   {                                                                   
     GyroUpdate();
+    TrackKeeper();
   }  
 }
 
   //linke Linie erreicht, nach rechts fahren
   if(lineValue[0] < 1000)                      
   {
-    motors.setSpeeds(maxSpeed, 0);
+    motors.setSpeeds(maxSpeed, maxSpeed/2);
     ledYellow(1);
     delay(200);    
     dir = 'R';
   //rechte Linie erreicht, nach links fahren
   else if(lineValue[2] < 1000)                 
   {
-    motors.setSpeeds(0, maxSpeed);  
+    motors.setSpeeds(maxSpeed/2, maxSpeed);  
     ledYellow(1); 
     delay(200); 
     dir = 'L';     
   else if(lineValue[1] < 1000)                 
   { 
     while(lineValue[0] > 1000)
-    motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
+    {
+      motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
+    }
     ledRed(1); 
     delay(1000); 
     dir = 'B';     
   if(randy == 1) 
   {
     //zur Kreuzungsmitte fahren
-    while((wayLeft != 1) && (wayRight != 1))
+    wayLeft = 0;
+    wayRight = 0;
+    while(wayLeft != 1 && wayRight != 1)
     {
       EncoderUpdate();
       motors.setSpeeds(maxSpeed/2, maxSpeed/2);
     GyroUpdate();
     while(rotationAngle != 90)
     {
-    motors.setSpeeds(0, maxSpeed);  
-    GyroUpdate();
+      GyroUpdate();
+      motors.setSpeeds(0, maxSpeed);  
     }
 
     //geradeaus fahren
     GyroUpdate();
     while(rotationAngle != 90)
     {
-    motors.setSpeeds(maxSpeed, 0);  
-    GyroUpdate();
+      GyroUpdate();
+      motors.setSpeeds(maxSpeed, 0);  
     }
 
     //geradeaus fahren
     motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
   }  
+
+  else motors.setSpeeds(maxSpeed, maxSpeed); 
 }
 
 //Funktion Serielle Ausgabe
   EncoderUpdate();
 
   //Funktionsauswahl
-/*   if((compass.a.x - compassOffset) > 1000) CollisionDetection();         
-  else if((proxValue[0] || proxValue[1]) > 4) ObstacleAvoidance();
-  else if((lineValue[0] < 1000)) && (lineValue[1] < 1000)) && (lineValue[2] < 1000))) Crossroad();
+/*   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 TrackKeeper(); */
 
   //Ausgabe über Bluetoothverbindung