Added functions moveSensorUpdate + encoderUpdate

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,19 +26,27 @@ uint16_t  lineOffset[3];
 
 uint8_t   proxValues[4];                //von links (0) nach rechts (3)
 
-uint32_t  turnAngle;                    //Drehwinkel
+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;
 
-uint4_t   rand;                         //Zufallszahl für Richtung
+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()                                            
 {
@@ -120,8 +130,9 @@ void setup()
   buttonA.waitForButton();
   LineSensorSetup();                                                
   TurnSensorSetup();
-  MoveSensorSetup();
   gyroLastUpdate = micros();
+  MoveSensorSetup();
+  compassLastUpdate = micros();  
   
   //Zumo bereit zu starten
   Serial1.println("Zumo ready to start");
@@ -130,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()                                            
 {
@@ -246,18 +300,6 @@ 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()
 {
@@ -265,17 +307,20 @@ void Crossroad()
   dir = 'A';
 
   //Zufallszahl erzeugen
-  rand = random(3);
+  randy = random(3);
 
   //Kodierung analysieren
 
 
   //links Abbiegen
-  if(rand == 1) 
+  if(randy == 1) 
   {
     //zur Kreuzungsmitte fahren
-    while((lineValues[0] < (lineOffset[0] + 200)) || (lineValues[1] < (lineOffset[1] + 200)) 
-    || (lineValues[2] < (lineOffset[2] + 200))) motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
+    while((countsLeft != 200) && (countsRight != 200))
+    {
+      EncoderUpdate();
+      motors.setSpeeds(maxSpeed/2, maxSpeed/2);
+    }
 
     //links drehen
     turnAngle = 0;
@@ -291,7 +336,7 @@ void Crossroad()
   }
 
   //rechts Abbiegen
-  else if(rand == 2) 
+  else if(randy == 2) 
   {
     //rechts drehen
     turnAngle = 0;
@@ -314,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);
@@ -331,15 +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((lineValues[0] > (lineOffset[0] + 200)) && (lineValues[1] > (lineOffset[1] + 200)) 
     && (lineValues[2] > (lineOffset[2] + 200))) Crossroad();
-  else TrackKeeper();
+  else TrackKeeper(); */
 
   //Ausgabe über Bluetoothverbindung
   SerialOutput();