Update Comments, Update Hindernisumfahrung (Gegenverkehr, Geschwindigkeiten)

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -1,7 +1,7 @@
 //Verfassser: Felix Stange MET2016
 //Datum: 19.07.2017 erstellt, 03.08.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). 
+//Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen). 
 //Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es 
 //dennoch zu einer Kollision, wird diese durch die Beschleunigungssensoren (LSM303) erkannt. 
 //Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G) 
@@ -15,7 +15,6 @@
 Zumo32U4ProximitySensors  proxSensors;  //Abstandssensoren
 Zumo32U4LineSensors       lineSensors;  //Liniensensoren
 Zumo32U4Motors            motors;
-Zumo32U4LCD               lcd;
 Zumo32U4ButtonA           buttonA;
 LSM303                    compass;      //Beschleunigungssensor x-Achse
 L3G                       gyro;         //Drehbewegungssensor z-Achse
@@ -34,17 +33,17 @@ int16_t   compassOffset;
 
 int       maxSpeed = 200;               //Maximale Geschwindigkeit (möglich 400)
 char      dir;                          //Fahrtrichtung, Ereignis
-char      report[120];                  //Ausgabe über Serial
+char      report[120];                  //Ausgabe
 
-void LineSensorSetup()                                            //Setup Liniensensoren
+//Setup Liniensensoren
+void LineSensorSetup()                                            
 {
-  lcd.clear();
-  lcd.print("Line cal");
   ledYellow(1);
   delay(500);
   
+  //Kalibrierung
   uint32_t total[3] = {0, 0, 0};  
-  for(uint8_t i = 0; i < 120; i++)                                //Kalibrierung
+  for(uint8_t i = 0; i < 120; i++)                                
   {
     if (i > 30 && i <= 90) motors.setSpeeds(200, -200);
     else motors.setSpeeds(-200, 200);
@@ -61,10 +60,9 @@ void LineSensorSetup()                                            //Setup Linien
   lineOffset[2] = total[2] / 120;
 }
 
-void TurnSensorSetup()                                            //Setup Drehbewegungssensor
+//Setup Drehbewegungssensor
+void TurnSensorSetup()                                            
 {                                                                 
-  lcd.clear();
-  lcd.print("Gyro cal");
   ledYellow(1);
   delay(500);
                                                                   //800Hz Ausgaberate
@@ -72,7 +70,8 @@ void TurnSensorSetup()                                            //Setup Drehbe
   gyro.writeReg(L3G::CTRL4, 0b00100000);                          //2000dps Auflösung
   gyro.writeReg(L3G::CTRL5, 0b00000000);                          //Hochpassfilter ausgeschaltet
 
-  int32_t total = 0;                                              //Kalibrierung
+  //Kalibrierung
+  int32_t total = 0;                                              
   for (uint16_t i = 0; i < 1024; i++)
   {
     while(!gyro.readReg(L3G::STATUS_REG) & 0x08);
@@ -83,14 +82,14 @@ void TurnSensorSetup()                                            //Setup Drehbe
   gyroOffset = total / 1024;
 }
 
-void MoveSensorSetup()                                            //Setup Beschleunigungssensor
+//Setup Beschleunigungssensor
+void MoveSensorSetup()                                            
 {
-  lcd.clear();
-  lcd.print("Acc cal");
   ledYellow(1);
   delay(500);
 
-  int32_t total = 0;                                              //Kalibrierung
+  //Kalibrierung
+  int32_t total = 0;                                              
   for (uint16_t i = 0; i < 1024; i++)
   {
     compass.read();
@@ -102,56 +101,55 @@ void MoveSensorSetup()                                            //Setup Beschl
 
 void setup() 
 {
-  lineSensors.initThreeSensors();                                   //Initialisierung der Sensoren
+  //Initialisierung der Bluetoothverbindung
+  Serial1.begin(9600);                                              
+  if(Serial1.available()) Serial1.println("bluetooth available");  
+
+  //Initialisierung der Sensoren
+  lineSensors.initThreeSensors();                                   
   proxSensors.initThreeSensors();
   Wire.begin();
   compass.init();
   compass.enableDefault();  
   gyro.init();
 
-  lcd.clear();                                                      //Kalibrierung der Sensoren
-  lcd.print("Press A");
-  lcd.gotoXY(0, 1);
-  lcd.print("to calib");
+  //Kalibrierung der Sensoren
+  Serial1.println("sensor calibration");                            
   buttonA.waitForButton();  
   LineSensorSetup();                                                
   TurnSensorSetup();
   MoveSensorSetup();
   gyroLastUpdate = micros();
   
-  lcd.clear();
-  lcd.print("Press A");
-  lcd.gotoXY(0, 1);
-  lcd.print("to start");
+  //Zumo bereit zu starten
+  Serial1.println("Zumo ready to start");
   buttonA.waitForButton();
 
-//  Serial1.begin(9600);
-//  if(Serial1.available()) Serial1.println("bluetooth available");  
-  Serial.begin(9600);
   delay(500);
 }
 
-void CollisionDetection()                                             //Funktion Kollisionserkennung
+ //Funktion Kollisionserkennung
+void CollisionDetection()                                            
 {
   dir = 'X';
-  motors.setSpeeds(0, 0);
+  Serial1.println("impact detected");
   ledRed(1);
-  lcd.clear();
-  lcd.print("Impact");
-  lcd.gotoXY(0, 1);
-  lcd.print("detected");
 
+  motors.setSpeeds(0, 0);
+  delay(500);
   motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
   delay(1000); 
-
-  SerialOutput();
 }
 
-void ObstacleAvoidance()                                              //Funktion Hindernisumfahrung
+//Funktion Hindernisumfahrung
+void ObstacleAvoidance()                                              
 {
   dir = 'U';
+  Serial1.println("obstacle avoidance");  
+  ledYellow(1);
 
-  motors.setSpeeds(maxSpeed/2, maxSpeed);                             //Schritt 1: links bis über Mittellinie fahren
+  //Schritt 1: links bis über Mittellinie fahren
+  motors.setSpeeds(maxSpeed/2, maxSpeed);                             
   turnAngle = 0;                                                      
   TurnSensorUpdate();                                                 
   while(lineValues[2] < (lineOffset[2] -200))                
@@ -159,23 +157,37 @@ void ObstacleAvoidance()                                              //Funktion
     lineSensors.read(lineValues);
   }
 
-  motors.setSpeeds(maxSpeed, maxSpeed/2);                             //Schritt 2: rechts fahren bis Fahrzeug gerade steht
-  while(turnAngle != 0)                                               //ohne dabei weitere Linien zu überfahren
+  //Schritt 2: rechts fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
+  motors.setSpeeds(maxSpeed, maxSpeed/2);                             
+  while(turnAngle != 0)                                               
   {                                                                   
     TurnSensorUpdate();
   }
 
-  motors.setSpeeds(maxSpeed, maxSpeed);                               //Schritt 3: geradeaus fahren bis Hindernis passiert
-  delay(1000);
-  proxSensors.read();
-  proxValues[3] = proxSensors.countsRightWithRightLeds();  
-  while(proxValues[3] > 4)
+  //Gegenverkehr beachten
+  proxSensors.read();                                                 
+  proxValues[1] = proxSensors.countsFrontWithLeftLeds();
+  proxValues[2] = proxSensors.countsFrontWithRightLeds();  
+  if((proxValues[1] || proxValues[2]) < 4)
   {
+    //Schritt 3: geradeaus fahren bis Hindernis passiert
+    maxSpeed = 400;
+    motors.setSpeeds(maxSpeed, maxSpeed);                             
+    delay(1000);
     proxSensors.read();
     proxValues[3] = proxSensors.countsRightWithRightLeds();  
+    while(proxValues[3] > 4)
+    {
+      proxSensors.read();
+      proxValues[3] = proxSensors.countsRightWithRightLeds();  
+      TrackKeeper();
+    }
+    maxSpeed = 200;
   }
   
-  motors.setSpeeds(maxSpeed/2, maxSpeed);                             //Schritt 4: rechts bis über Mittellinie fahren
+  //Schritt 4: rechts bis über Mittellinie fahren
+  motors.setSpeeds(maxSpeed/2, maxSpeed);                             
+  ledYellow(1);
   turnAngle = 0;                                                      
   TurnSensorUpdate();                                                 
   while(lineValues[0] < (lineOffset[0] -200))                
@@ -183,38 +195,48 @@ void ObstacleAvoidance()                                              //Funktion
     lineSensors.read(lineValues);
   }
 
-  motors.setSpeeds(maxSpeed/2, maxSpeed);                             //Schritt 5: links fahren bis Fahrzeug gerade steht
-  while(turnAngle != 0)                                               //ohne dabei weitere Linien zu überfahren
+  //Schritt 5: links fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
+  motors.setSpeeds(maxSpeed/2, maxSpeed);                             
+  while(turnAngle != 0)                                               
   {                                                                   
     TurnSensorUpdate();
   }  
 
-  SerialOutput();
+  ledYellow(0);
 }
 
-void TrackKeeper()                                                   //Funktion Spurhalten
+//Funktion Spurhalten
+void TrackKeeper()                                                   
 {
-  if(lineValues[0] < (lineOffset[0] - 200))                          //linke Linie erreicht, nach rechts fahren
+
+  //linke Linie erreicht, nach rechts fahren
+  if(lineValues[0] < (lineOffset[0] - 200))                          
   {
     motors.setSpeeds(maxSpeed, 0);
     ledYellow(1);
     delay(200);    
     dir = 'R';
   }
-  else if(lineValues[2] < (lineOffset[2] - 200))                     //rechte Linie erreicht, nach links fahren
+
+  //rechte Linie erreicht, nach links fahren
+  else if(lineValues[2] < (lineOffset[2] - 200))                     
   {
     motors.setSpeeds(0, maxSpeed);  
     ledYellow(1); 
     delay(200); 
     dir = 'L';     
   }
-  else if(lineValues[1] < (lineOffset[1] - 100))                     //Linie überfahren, zurücksetzen
+
+  //Linie überfahren, zurücksetzen
+  else if(lineValues[1] < (lineOffset[1] - 100))                     
   { 
     motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
     ledRed(1); 
     delay(1000); 
     dir = 'B';     
   }  
+
+  //Normale Fahrt
   else                                                                
   {
     motors.setSpeeds(maxSpeed, maxSpeed);    
@@ -222,11 +244,10 @@ void TrackKeeper()                                                   //Funktion
     delay(100);
     dir = 'F';     
   }
-
-  SerialOutput();
 }
 
-void TurnSensorUpdate()                                                 //Funktion Drehbewegungssensor
+//Funktion Drehbewegungssensor
+void TurnSensorUpdate()                                                 
 {
   gyro.read();
   turnRate = gyro.g.z - gyroOffset;
@@ -237,15 +258,21 @@ void TurnSensorUpdate()                                                 //Funkti
   turnAngle += (int64_t)d * 14680064 / 17578125;
 }
 
-void SerialOutput()                                                     //Funktion Serielle Ausgabe
+//Funktion Abbiegen
+void Crossroad()
+{
+
+}
+
+//Funktion Serielle Ausgabe
+void SerialOutput()                                                     
 {
   snprintf_P(report, sizeof(report),
-    PSTR("Abstand: %3d %3d   Linien: %6d %6d %6d   Richtung: %3c   Beschleunigung: %6d   Drehwinkel: %6d"),
+    PSTR("Abstand: %3d %3d %3d %3d   Linie: %6d %6d %6d   Richtung: %3c   Beschleunigung: %6d   Winkel: %6d"),
     proxValues[0], proxValues[1], proxValues[2], proxValues[3], 
     lineValues[0], lineValues[1], lineValues[2],
     dir, compass.a.x, gyro.g.z);
-  Serial.println(report);
-//  if(Serial1.available()) Serial1.println(report);
+  Serial1.println(report);
 }
 
 void loop() 
@@ -253,9 +280,9 @@ void loop()
   ledGreen(0);
   ledYellow(0);
   ledRed(0);
-  lcd.clear();
 
-  lineSensors.read(lineValues);                                           //Abfragen der Sensordaten
+  //Abfragen der Sensordaten
+  lineSensors.read(lineValues);                                           
   proxSensors.read();
   proxValues[0] = proxSensors.countsLeftWithLeftLeds();
   proxValues[1] = proxSensors.countsFrontWithLeftLeds();
@@ -264,7 +291,12 @@ void loop()
   compass.read();
   gyro.read();
 
-  if((compass.a.x - compassOffset) > 16000) CollisionDetection();         //Funktionsauswahl
+  //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 TrackKeeper();
+
+  //Ausgabe über Bluetoothverbindung
+  SerialOutput();                                                         
 }