bugfixing SensorUpdates

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -1,7 +1,7 @@
 //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) 
@@ -29,12 +29,14 @@ uint8_t   proxValues[4];                //von links (0) nach rechts (3)
 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;
@@ -43,7 +45,7 @@ uint8_t   randy;                        //Zufallszahl für Richtung
 
 int       maxSpeed = 200;               //Maximale Geschwindigkeit (möglich 400)
 char      dir = '0';                    //Fahrtrichtung, Ereignis
-char      report[120];                  //Ausgabe
+char      report[200];                  //Ausgabe
 
 /*-----------------------------------------------------------------*/
 
@@ -51,7 +53,6 @@ char      report[120];                  //Ausgabe
 void LineSensorSetup()                                            
 {
   ledYellow(1);
-  delay(500);
   
   //Kalibrierung
   uint32_t total[3] = {0, 0, 0};  
@@ -76,7 +77,6 @@ void LineSensorSetup()
 void TurnSensorSetup()                                            
 {                                                                 
   ledYellow(1);
-  delay(500);
                                                                   //800Hz Ausgaberate
   gyro.writeReg(L3G::CTRL1, 0b11111111);                          //Tiefpassfilter bei 100Hz
   gyro.writeReg(L3G::CTRL4, 0b00100000);                          //2000dps Auflösung
@@ -98,7 +98,6 @@ void TurnSensorSetup()
 void MoveSensorSetup()                                            
 {
   ledYellow(1);
-  delay(500);
 
   //Kalibrierung
   int32_t total = 0;                                              
@@ -126,8 +125,8 @@ void setup()
   gyro.init();
 
   //Kalibrierung der Sensoren
-  Serial1.println("sensor calibration");                            
-  buttonA.waitForButton();
+  Serial1.println("sensor calibration, dont touch");                            
+  delay(500);
   LineSensorSetup();                                                
   TurnSensorSetup();
   gyroLastUpdate = micros();
@@ -135,9 +134,8 @@ void setup()
   compassLastUpdate = micros();  
   
   //Zumo bereit zu starten
-  Serial1.println("Zumo ready to start");
+  Serial1.println("Zumo ready to start, press A");
   buttonA.waitForButton();
-
   delay(500);
 }
 
@@ -153,7 +151,7 @@ void TurnSensorUpdate()
   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
@@ -165,8 +163,8 @@ void MoveSensorUpdate()
   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
@@ -353,13 +351,22 @@ void Crossroad()
 }
 
 //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);
 }