Update

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -1,5 +1,5 @@
 //Verfassser: Felix Stange 4056379 MET2016
-//Datum: 19.07.2017 erstellt, 25.10.2017 zuletzt geändert
+//Datum: 19.07.2017 erstellt, 08.12.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 
@@ -42,10 +42,11 @@ int16_t   moveRate;
 int16_t   compassOffset;                
 uint16_t  compassLastUpdate;
 
+uint16_t  LastUpdate;
 int       maxSpeed = 200;               //Maximale Geschwindigkeit (möglich 400)
-char      dir = "0";                    //Fahrtrichtung, Ereignis
-char      report[200];                  //Ausgabe
-char      attention;                    //zeigt Überhol-/Abbiegevorgang an
+char      dir = 'O';                    //Fahrtrichtung, Ereignis
+char      report[250];                  //Ausgabe
+int       warning = 0;                  //1 zeigt Überhol-/Abbiegevorgang an
 
 /*-----------------------------------------------------------------*/
 
@@ -130,12 +131,13 @@ void setup()
 {
   //Initialisierung der Bluetoothverbindung
   Serial1.begin(9600);                                              
-  if(Serial1.available()) Serial1.println("Bluetooth verfügbar");  
+  if(Serial1.available()) Serial1.println("Verbindung hergestellt");  
 
   //Initialisierung und Kalibrierung der Sensoren
   Serial1.println("Sensorkalibrierung");   
   Wire.begin();                       
   delay(500);
+  LastUpdate = micros();
   ProxSetup();
   LineSetup();                                                
   GyroSetup();
@@ -151,6 +153,14 @@ void setup()
 
 /*-----------------------------------------------------------------*/
 
+//Update Durchlaufzeit
+void TimeUpdate()
+{
+  uint16_t m = micros();
+  uint16_t dt = m - LastUpdate;
+  LastUpdate = m;
+}
+
 //Update Abstandssensoren
 void ProxUpdate()
 {
@@ -210,7 +220,7 @@ void EncoderUpdate()
  //Funktion Kollisionserkennung
 void CollisionDetection()                                            
 {
-  dir = "X";
+  dir = 'X';
   Serial1.println("Aufprall erkannt");
   ledRed(1);
 
@@ -223,8 +233,9 @@ void CollisionDetection()
 //Funktion Hindernisumfahrung
 void ObstacleAvoidance()                                              
 {
-  dir = "U";
-  Serial1.println("Überholen");  
+  dir = 'U';
+  Serial1.println("Überholen"); 
+  Serial1.println("1"); 
   ledYellow(1);
 
   //Schritt 1: Spurwechsel links   
@@ -311,7 +322,7 @@ void TrackKeeper()
     motors.setSpeeds(maxSpeed, maxSpeed/2);
     ledYellow(1);
     delay(100);    
-    dir = "R";
+    dir = 'R';
   }
 
   //rechte Linie erreicht, nach links fahren
@@ -320,7 +331,7 @@ void TrackKeeper()
     motors.setSpeeds(maxSpeed/2, maxSpeed);  
     ledYellow(1); 
     delay(100); 
-    dir = "L";     
+    dir = 'L';     
   }
 
   //Linie überfahren, zurücksetzen
@@ -332,7 +343,7 @@ void TrackKeeper()
       motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
     }
     delay(500); 
-    dir = "B";     
+    dir = 'B';     
   }  
 
   //normale Fahrt
@@ -340,7 +351,7 @@ void TrackKeeper()
   {
     motors.setSpeeds(maxSpeed, maxSpeed);    
     ledGreen(1); 
-    dir = "F";     
+    dir = 'F';     
   }
 }
 
@@ -348,8 +359,9 @@ void TrackKeeper()
 void Crossroad()
 {
   ledYellow(1); 
-  dir = "A";
+  dir = 'A';
   Serial1.println("Abbiegen");  
+  Serial1.println("1"); 
 
   //Kodierung analysieren
   bool  leftCode;                                                         //links => 1
@@ -386,7 +398,9 @@ void Crossroad()
     while(rotationAngle != 90)
     {
       GyroUpdate();
-      motors.setSpeeds(0, maxSpeed/2);  
+      if(lineValue[0] < 1000) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
+      else if(lineValue[2] < 1000) motors.setSpeeds(0, maxSpeed/2);
+      else motors.setSpeeds(maxSpeed/4, maxSpeed/2);  
     }
     //geradeaus fahren
     motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
@@ -401,7 +415,9 @@ void Crossroad()
     while(rotationAngle != -90)
     {
       GyroUpdate();
-      motors.setSpeeds(maxSpeed/2, 0);  
+      if(lineValue[0] < 1000) motors.setSpeeds(maxSpeed/2, 0);
+      else if(lineValue[2] < 1000) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
+      else motors.setSpeeds(maxSpeed/2, maxSpeed/4);  
     }
     //geradeaus fahren
     motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
@@ -417,11 +433,11 @@ void Crossroad()
 void SerialOutput()                                                     
 {
   snprintf_P(report, sizeof(report),
-    PSTR("Abstand: %3d %3d %3d %3d   Linie: %6d %6d %6d   Richtung: %3c   Weg: %6d %6d   Winkel: %6d"),
+    PSTR("Abstand: %3d %3d %3d %3d   Linie: %6d %6d %6d   Weg: %3d %3d   Winkel: %3d   Ereignis: %3c   Zeit: %3d"),
     proxValue[0], proxValue[1], proxValue[2], proxValue[3], 
     lineValue[0], lineValue[1], lineValue[2],
-    dir, driveRotation[0], driveRotation[1], rotationAngle);
-  Serial1.println(report);
+    driveRotation[0], driveRotation[1], rotationAngle, dir, LastUpdate);
+  Serial.println(report);
 }
 
 /*-----------------------------------------------------------------*/
@@ -433,6 +449,7 @@ void loop()
   ledRed(0);
 
   //Abfragen der Sensordaten
+  TimeUpdate();
   LineUpdate();                                         
   ProxUpdate();
   GyroUpdate();
@@ -440,11 +457,11 @@ void loop()
   EncoderUpdate();
 
   //Funktionsauswahl
-  attention = Serial1.read();
-  if(attention == "Überholen" || "Abbiegen") 
+  warning = Serial1.read();
+  if(warning == 1) 
   {
     maxSpeed = 100;
-    if(moveRate > 1000 || proxValue[0] > 4 || proxValue[1] > 4 || lineValue[0] > 500 && lineValue[0] < 1500) || 
+    if(moveRate > 1000 || proxValue[0] > 4 || proxValue[1] > 4 || lineValue[0] > 500 && lineValue[0] < 1500 || 
     lineValue[2] > 500 && lineValue[2] < 1500) motors.setSpeeds(0, 0);  
     else TrackKeeper();
   }