bugfixing SensorUpdates

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

 //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). 
+//Liniensensoren (3), ähnlich einer Spurhalteautomatik (dunkler Belag und helle 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) 
 int16_t   countsLeft;                   //Encoder
 int16_t   countsRight;
 
-uint16_t  turnAngle = 0;                //Drehwinkel
+int32_t   rotationAngle = 0;            //Drehwinkel
+int32_t   turnAngle = 0; 
 int16_t   turnRate;
 int16_t   gyroOffset;
 uint16_t  gyroLastUpdate;
 
-uint16_t  moveDistance = 0;             //Beschleunigung
+int32_t   moveDisplay = 0;              //Beschleunigung
+int32_t   moveDistance = 0;   
 int16_t   moveRate;
 int16_t   compassOffset;                
 uint16_t  compassLastUpdate;
 
 int       maxSpeed = 200;               //Maximale Geschwindigkeit (möglich 400)
 char      dir = '0';                    //Fahrtrichtung, Ereignis
-char      report[120];                  //Ausgabe
+char      report[200];                  //Ausgabe
 
 /*-----------------------------------------------------------------*/
 
 void LineSensorSetup()                                            
 {
   ledYellow(1);
-  delay(500);
   
   //Kalibrierung
   uint32_t total[3] = {0, 0, 0};  
 void TurnSensorSetup()                                            
 {                                                                 
   ledYellow(1);
-  delay(500);
                                                                   //800Hz Ausgaberate
   gyro.writeReg(L3G::CTRL1, 0b11111111);                          //Tiefpassfilter bei 100Hz
   gyro.writeReg(L3G::CTRL4, 0b00100000);                          //2000dps Auflösung
 void MoveSensorSetup()                                            
 {
   ledYellow(1);
-  delay(500);
 
   //Kalibrierung
   int32_t total = 0;                                              
   gyro.init();
 
   //Kalibrierung der Sensoren
-  Serial1.println("sensor calibration");                            
-  buttonA.waitForButton();
+  Serial1.println("sensor calibration, dont touch");                            
+  delay(500);
   LineSensorSetup();                                                
   TurnSensorSetup();
   gyroLastUpdate = micros();
   compassLastUpdate = micros();  
   
   //Zumo bereit zu starten
-  Serial1.println("Zumo ready to start");
+  Serial1.println("Zumo ready to start, press A");
   buttonA.waitForButton();
-
   delay(500);
 }
 
   gyroLastUpdate = m;
   int32_t d = (int32_t)turnRate * dt;
   turnAngle += (int64_t)d * 14680064 / 17578125;
-  turnAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
+  rotationAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
 }
 
 // Update Beschleunigungssensor
   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;
+  moveDistance += (int64_t)d * dt >> 14;
+  moveDisplay = moveDistance * 1000 / 9,81;
 }
 
 //Update Encoder
 }
 
 //Funktion Serielle Ausgabe
-void SerialOutput()                                                     
+/* void SerialOutput()                                                     
 {
   snprintf_P(report, sizeof(report),
     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, moveDistance, turnAngle);
+    dir, moveDisplay, rotationAngle);
+  Serial1.println(report);
+} */
+
+void SerialOutput()                                                     
+{
+  snprintf_P(report, sizeof(report),
+    PSTR("Winkel: %6d %6d %6d %6d %6u   Weg: %6d %6d %6d %6d %6u"),
+    rotationAngle, turnAngle, turnRate, gyroOffset, gyroLastUpdate, 
+    moveDisplay, moveDistance, moveRate, compassOffset, compassLastUpdate);
   Serial1.println(report);
 }