README.md

@@ -3,7 +3,7 @@
 
 Es werden „Zumo 32U4 Robot“ der Firma „Pololu“ bereitgestellt. Diese verfügen über einen Antrieb, sowie eine Vielzahl von Sensoren. Die Programmierung erfolgt mithilfe der Arduino IDE. Mit diesen Roboterfahrzeugen sollen folgende Aufgaben realisiert werden: 
 
-- Anpassen der Geschwindigkeit entsprechend der     Fahrsituation, z.B. langsames Heranfahren an Hindernisse 
+- Anpassen der Geschwindigkeit entsprechend der Fahrsituation, z.B. langsames Heranfahren an Hindernisse 
 - Vor dem Abbiegen Geschwindigkeit reduzieren
 - Automatischer gegenläufiger Fahrbetrieb auf einer mehrspurigen Straße mit angepasster Geschwindigkeit
 - Anfertigung einer geeigneten Fahrstrecke mit Abbiegemöglichkeit

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -1,11 +1,12 @@
-//Verfassser: Felix Stange MET2016
-//Datum: 19.07.2017 erstellt, 03.08.2017 zuletzt geändert
+//Verfassser: Felix Stange 4056379 MET2016
+//Datum: 19.07.2017 erstellt, 11.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 (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) 
-//genutzt. Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die 
+//genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden. 
+//Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die 
 //Kommunikation erfolgt seriell ('SERIAL' für USB, 'SERIAL1' für Bluetooth). 
 //das LCD kann bei Bluetoothkommunikation nicht genutzt werden. 
 
@@ -16,6 +17,7 @@ Zumo32U4ProximitySensors  proxSensors;  //Abstandssensoren
 Zumo32U4LineSensors       lineSensors;  //Liniensensoren
 Zumo32U4Motors            motors;
 Zumo32U4ButtonA           buttonA;
+Zumo32U4Encoders          encoders;
 LSM303                    compass;      //Beschleunigungssensor x-Achse
 L3G                       gyro;         //Drehbewegungssensor z-Achse
 
@@ -24,17 +26,27 @@ uint16_t  lineOffset[3];
 
 uint8_t   proxValues[4];                //von links (0) nach rechts (3)
 
-uint32_t  turnAngle;                    //Drehwinkel in °
+int16_t   countsLeft;                   //Encoder
+int16_t   countsRight;
+
+uint16_t  turnAngle = 0;                //Drehwinkel
 int16_t   turnRate;
 int16_t   gyroOffset;
 uint16_t  gyroLastUpdate;
 
-int16_t   compassOffset;
+uint16_t  moveDistance = 0;             //Beschleunigung
+int16_t   moveRate;
+int16_t   compassOffset;                
+uint16_t  compassLastUpdate;
+
+uint8_t   randy;                        //Zufallszahl für Richtung
 
 int       maxSpeed = 200;               //Maximale Geschwindigkeit (möglich 400)
-char      dir;                          //Fahrtrichtung, Ereignis
+char      dir = '0';                    //Fahrtrichtung, Ereignis
 char      report[120];                  //Ausgabe
 
+/*-----------------------------------------------------------------*/
+
 //Setup Liniensensoren
 void LineSensorSetup()                                            
 {
@@ -115,11 +127,12 @@ void setup()
 
   //Kalibrierung der Sensoren
   Serial1.println("sensor calibration");                            
-  buttonA.waitForButton();  
+  buttonA.waitForButton();
   LineSensorSetup();                                                
   TurnSensorSetup();
-  MoveSensorSetup();
   gyroLastUpdate = micros();
+  MoveSensorSetup();
+  compassLastUpdate = micros();  
   
   //Zumo bereit zu starten
   Serial1.println("Zumo ready to start");
@@ -128,6 +141,49 @@ void setup()
   delay(500);
 }
 
+/*-----------------------------------------------------------------*/
+
+//Update Drehbewegungssensor
+void TurnSensorUpdate()                                                 
+{
+  gyro.read();
+  turnRate = gyro.g.z - gyroOffset;
+  uint16_t m = micros();
+  uint16_t dt = m - gyroLastUpdate;
+  gyroLastUpdate = m;
+  int32_t d = (int32_t)turnRate * dt;
+  turnAngle += (int64_t)d * 14680064 / 17578125;
+  turnAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
+}
+
+// Update Beschleunigungssensor
+void MoveSensorUpdate()
+{
+  compass.read();
+  moveRate = compass.a.x - compassOffset;
+  uint16_t m = micros();
+  uint16_t dt = m - compassLastUpdate;
+  compassLastUpdate = m;
+  int32_t d = (int32_t)moveRate * dt;
+  moveDistance += (int64_t)d * dt / 16384;
+  moveDistance = (moveDistance * 1000) / 9,81;
+}
+
+//Update Encoder
+//12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
+void EncoderUpdate()
+{
+  static uint8_t lastDisplayTime;
+  if ((uint8_t)(millis() - lastDisplayTime) >= 100)
+  {
+    lastDisplayTime = millis();
+    countsLeft = encoders.getCountsLeft();
+    countsRight = encoders.getCountsRight();
+  }
+}
+
+/*-----------------------------------------------------------------*/
+
  //Funktion Kollisionserkennung
 void CollisionDetection()                                            
 {
@@ -201,8 +257,6 @@ void ObstacleAvoidance()
   {                                                                   
     TurnSensorUpdate();
   }  
-
-  ledYellow(0);
 }
 
 //Funktion Spurhalten
@@ -210,7 +264,7 @@ void TrackKeeper()
 {
 
   //linke Linie erreicht, nach rechts fahren
-  if(lineValues[0] < (lineOffset[0] - 200))                          
+  if(lineValues[0] > (lineOffset[0] + 200))                          
   {
     motors.setSpeeds(maxSpeed, 0);
     ledYellow(1);
@@ -219,7 +273,7 @@ void TrackKeeper()
   }
 
   //rechte Linie erreicht, nach links fahren
-  else if(lineValues[2] < (lineOffset[2] - 200))                     
+  else if(lineValues[2] > (lineOffset[2] + 200))                     
   {
     motors.setSpeeds(0, maxSpeed);  
     ledYellow(1); 
@@ -228,7 +282,7 @@ void TrackKeeper()
   }
 
   //Linie überfahren, zurücksetzen
-  else if(lineValues[1] < (lineOffset[1] - 100))                     
+  else if(lineValues[1] > (lineOffset[1] + 200))                     
   { 
     motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
     ledRed(1); 
@@ -246,22 +300,56 @@ void TrackKeeper()
   }
 }
 
-//Funktion Drehbewegungssensor
-void TurnSensorUpdate()                                                 
-{
-  gyro.read();
-  turnRate = gyro.g.z - gyroOffset;
-  uint16_t m = micros();
-  uint16_t dt = m - gyroLastUpdate;
-  gyroLastUpdate = m;
-  int32_t d = (int32_t)turnRate * dt;
-  turnAngle += (int64_t)d * 14680064 / 17578125;
-}
-
 //Funktion Abbiegen
 void Crossroad()
 {
+  ledYellow(1); 
+  dir = 'A';
+
+  //Zufallszahl erzeugen
+  randy = random(3);
 
+  //Kodierung analysieren
+
+
+  //links Abbiegen
+  if(randy == 1) 
+  {
+    //zur Kreuzungsmitte fahren
+    while((countsLeft != 200) && (countsRight != 200))
+    {
+      EncoderUpdate();
+      motors.setSpeeds(maxSpeed/2, maxSpeed/2);
+    }
+
+    //links drehen
+    turnAngle = 0;
+    TurnSensorUpdate();
+    while(turnAngle != 90)
+    {
+    motors.setSpeeds(0, maxSpeed);  
+    TurnSensorUpdate();
+    }
+
+    //geradeaus fahren
+    motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
+  }
+
+  //rechts Abbiegen
+  else if(randy == 2) 
+  {
+    //rechts drehen
+    turnAngle = 0;
+    TurnSensorUpdate();
+    while(turnAngle != 90)
+    {
+    motors.setSpeeds(maxSpeed, 0);  
+    TurnSensorUpdate();
+    }
+
+    //geradeaus fahren
+    motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
+  }  
 }
 
 //Funktion Serielle Ausgabe
@@ -271,10 +359,12 @@ void SerialOutput()
     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);
+    dir, moveDistance, turnAngle);
   Serial1.println(report);
 }
 
+/*-----------------------------------------------------------------*/
+
 void loop() 
 {
   ledGreen(0);
@@ -288,14 +378,15 @@ void loop()
   proxValues[1] = proxSensors.countsFrontWithLeftLeds();
   proxValues[2] = proxSensors.countsFrontWithRightLeds();  
   proxValues[3] = proxSensors.countsRightWithRightLeds();  
-  compass.read();
-  gyro.read();
+  TurnSensorUpdate();
+  MoveSensorUpdate();
 
   //Funktionsauswahl
-  if((compass.a.x - compassOffset) > 16000) CollisionDetection();         
+/*   if((compass.a.x - compassOffset) > 4000) CollisionDetection();         
   else if((proxValues[0] || proxValues[1]) > 4) ObstacleAvoidance();
-  else if (lineValue[0] && lineValue[1] && lineValue[2]) Crossroad();
-  else TrackKeeper();
+  else if((lineValues[0] > (lineOffset[0] + 200)) && (lineValues[1] > (lineOffset[1] + 200)) 
+    && (lineValues[2] > (lineOffset[2] + 200))) Crossroad();
+  else TrackKeeper(); */
 
   //Ausgabe über Bluetoothverbindung
   SerialOutput();