General improvements

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -8,7 +8,7 @@
 //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. 
+//Das LCD kann bei Bluetoothnutzung nicht verwendet werden. 
 
 #include <Wire.h>
 #include <Zumo32U4.h>
@@ -21,13 +21,16 @@ Zumo32U4Encoders          encoders;
 LSM303                    compass;      //Beschleunigungssensor x-Achse
 L3G                       gyro;         //Drehbewegungssensor z-Achse
 
-uint16_t  lineValues[3];                //von links (0) nach rechts (2)
+uint16_t  lineValue[3];                //von links (0) nach rechts (2)
 uint16_t  lineOffset[3]; 
+uint16_t  lineRaw[3];
 
-uint8_t   proxValues[4];                //von links (0) nach rechts (3)
+uint8_t   proxValue[4];                //von links (0) nach rechts (3)
 
-int16_t   countsLeft;                   //Encoder
-int16_t   countsRight;
+int16_t   countsLeft;                   //Encoder rechts
+int16_t   wayLeft;
+int16_t   countsRight;                  //Encoder links
+int16_t   wayRight;
 
 int32_t   rotationAngle = 0;            //Drehwinkel
 int32_t   turnAngle = 0; 
@@ -50,16 +53,17 @@ char      report[200];                  //Ausgabe
 /*-----------------------------------------------------------------*/
 
 //Setup Liniensensoren
-void LineSensorSetup()                                            
+void LineSetup()                                            
 {
   ledYellow(1);
+  lineSensors.initThreeSensors();     
   
   //Kalibrierung
   uint32_t total[3] = {0, 0, 0};  
   for(uint8_t i = 0; i < 120; i++)                                
   {
-    if (i > 30 && i <= 90) motors.setSpeeds(200, -200);
-    else motors.setSpeeds(-200, 200);
+    if (i > 30 && i <= 90) motors.setSpeeds(maxSpeed, -maxSpeed);
+    else motors.setSpeeds(-maxSpeed, maxSpeed);
     lineSensors.read(lineOffset);  
     total[0] += lineOffset[0];
     total[1] += lineOffset[1];
@@ -73,12 +77,19 @@ void LineSensorSetup()
   lineOffset[2] = total[2] / 120;
 }
 
+//Setup Abstandssensoren
+void ProxSetup()
+{
+  proxSensors.initThreeSensors();  
+}
+
 //Setup Drehbewegungssensor
-void TurnSensorSetup()                                            
+void GyroSetup()                                            
 {                                                                 
   ledYellow(1);
-                                                                  //800Hz Ausgaberate
-  gyro.writeReg(L3G::CTRL1, 0b11111111);                          //Tiefpassfilter bei 100Hz
+  gyro.init();
+
+  gyro.writeReg(L3G::CTRL1, 0b11111111);                          //Ausgaberate 800Hz, Tiefpassfilter 100Hz
   gyro.writeReg(L3G::CTRL4, 0b00100000);                          //2000dps Auflösung
   gyro.writeReg(L3G::CTRL5, 0b00000000);                          //Hochpassfilter ausgeschaltet
 
@@ -90,14 +101,18 @@ void TurnSensorSetup()
     gyro.read();
     total += gyro.g.z;
   }
-  ledYellow(0);
   gyroOffset = total / 1024;
+
+  gyroLastUpdate = micros();
+  ledYellow(0);
 }
 
 //Setup Beschleunigungssensor
-void MoveSensorSetup()                                            
+void CompassSetup()                                            
 {
   ledYellow(1);
+  compass.init();
+  compass.enableDefault();  
 
   //Kalibrierung
   int32_t total = 0;                                              
@@ -106,32 +121,28 @@ void MoveSensorSetup()
     compass.read();
     total += compass.a.x;
   }
-  ledYellow(0);
   compassOffset = total / 1024;
+
+  compassLastUpdate = micros();  
+  ledYellow(0);
 }
 
+/*-----------------------------------------------------------------*/
+
 void setup() 
 {
   //Initialisierung der Bluetoothverbindung
   Serial1.begin(9600);                                              
   if(Serial1.available()) Serial1.println("bluetooth available");  
 
-  //Initialisierung der Sensoren
-  lineSensors.initThreeSensors();                                   
-  proxSensors.initThreeSensors();
-  Wire.begin();
-  compass.init();
-  compass.enableDefault();  
-  gyro.init();
-
-  //Kalibrierung der Sensoren
-  Serial1.println("sensor calibration, dont touch");                            
+  //Initialisierung und Kalibrierung der Sensoren
+  Serial1.println("sensor calibration, dont touch");   
+  Wire.begin();                       
   delay(500);
-  LineSensorSetup();                                                
-  TurnSensorSetup();
-  gyroLastUpdate = micros();
-  MoveSensorSetup();
-  compassLastUpdate = micros();  
+  ProxSetup();
+  LineSetup();                                                
+  GyroSetup();
+  CompassSetup();
   
   //Zumo bereit zu starten
   Serial1.println("Zumo ready to start, press A");
@@ -141,8 +152,27 @@ void setup()
 
 /*-----------------------------------------------------------------*/
 
+//Update Abstandssensoren
+void ProxUpdate()
+{
+  proxSensors.read();
+  proxValue[0] = proxSensors.countsLeftWithLeftLeds();
+  proxValue[1] = proxSensors.countsFrontWithLeftLeds();
+  proxValue[2] = proxSensors.countsFrontWithRightLeds();  
+  proxValue[3] = proxSensors.countsRightWithRightLeds(); 
+}
+
+//Updaten Liniensensoren
+void LineUpdate()
+{
+  lineSensors.read(lineRaw);
+  lineValue[0] = lineRaw[0] - lineOffset[0];
+  lineValue[1] = lineRaw[1] - lineOffset[1];
+  lineValue[2] = lineRaw[2] - lineOffset[2];
+}
+
 //Update Drehbewegungssensor
-void TurnSensorUpdate()                                                 
+void GyroUpdate()                                                 
 {
   gyro.read();
   turnRate = gyro.g.z - gyroOffset;
@@ -155,7 +185,7 @@ void TurnSensorUpdate()
 }
 
 // Update Beschleunigungssensor
-void MoveSensorUpdate()
+void CompassUpdate()
 {
   compass.read();
   moveRate = compass.a.x - compassOffset;
@@ -168,16 +198,13 @@ void MoveSensorUpdate()
 }
 
 //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();
-  }
+  countsLeft = encoders.getCountsLeft();
+  wayLeft = countsRight / 910;                            //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
+  countsRight = encoders.getCountsRight();
+  wayRight = countsRight / 910;
 }
 
 /*-----------------------------------------------------------------*/
@@ -204,36 +231,36 @@ void ObstacleAvoidance()
 
   //Schritt 1: links bis über Mittellinie fahren
   motors.setSpeeds(maxSpeed/2, maxSpeed);                             
-  turnAngle = 0;                                                      
-  TurnSensorUpdate();                                                 
-  while(lineValues[2] < (lineOffset[2] -200))                
+  rotationAngle = 0;                                                      
+  GyroUpdate();                                                 
+  while(lineValue[2] > 1000)              
   {
-    lineSensors.read(lineValues);
+    LineUpdate();
   }
 
   //Schritt 2: rechts fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
   motors.setSpeeds(maxSpeed, maxSpeed/2);                             
-  while(turnAngle != 0)                                               
+  while(rotationAngle != 0)                                               
   {                                                                   
-    TurnSensorUpdate();
+    GyroUpdate();
   }
 
   //Gegenverkehr beachten
   proxSensors.read();                                                 
-  proxValues[1] = proxSensors.countsFrontWithLeftLeds();
-  proxValues[2] = proxSensors.countsFrontWithRightLeds();  
-  if((proxValues[1] || proxValues[2]) < 4)
+  proxValue[1] = proxSensors.countsFrontWithLeftLeds();
+  proxValue[2] = proxSensors.countsFrontWithRightLeds();  
+  if((proxValue[1] || proxValue[2]) < 4)
   {
     //Schritt 3: geradeaus fahren bis Hindernis passiert
     maxSpeed = 400;
     motors.setSpeeds(maxSpeed, maxSpeed);                             
     delay(1000);
     proxSensors.read();
-    proxValues[3] = proxSensors.countsRightWithRightLeds();  
-    while(proxValues[3] > 4)
+    proxValue[3] = proxSensors.countsRightWithRightLeds();  
+    while(proxValue[3] > 4)
     {
       proxSensors.read();
-      proxValues[3] = proxSensors.countsRightWithRightLeds();  
+      proxValue[3] = proxSensors.countsRightWithRightLeds();  
       TrackKeeper();
     }
     maxSpeed = 200;
@@ -242,18 +269,18 @@ void ObstacleAvoidance()
   //Schritt 4: rechts bis über Mittellinie fahren
   motors.setSpeeds(maxSpeed/2, maxSpeed);                             
   ledYellow(1);
-  turnAngle = 0;                                                      
-  TurnSensorUpdate();                                                 
-  while(lineValues[0] < (lineOffset[0] -200))                
+  rotationAngle = 0;                                                      
+  GyroUpdate();                                                 
+  while(lineValue[0] - 200)     
   {
-    lineSensors.read(lineValues);
+    LineUpdate();
   }
 
   //Schritt 5: links fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
   motors.setSpeeds(maxSpeed/2, maxSpeed);                             
-  while(turnAngle != 0)                                               
+  while(rotationAngle != 0)                                               
   {                                                                   
-    TurnSensorUpdate();
+    GyroUpdate();
   }  
 }
 
@@ -262,7 +289,7 @@ void TrackKeeper()
 {
 
   //linke Linie erreicht, nach rechts fahren
-  if(lineValues[0] > (lineOffset[0] + 200))                          
+  if(lineValue[0] < 1000)                      
   {
     motors.setSpeeds(maxSpeed, 0);
     ledYellow(1);
@@ -271,7 +298,7 @@ void TrackKeeper()
   }
 
   //rechte Linie erreicht, nach links fahren
-  else if(lineValues[2] > (lineOffset[2] + 200))                     
+  else if(lineValue[2] < 1000)                 
   {
     motors.setSpeeds(0, maxSpeed);  
     ledYellow(1); 
@@ -280,8 +307,9 @@ void TrackKeeper()
   }
 
   //Linie überfahren, zurücksetzen
-  else if(lineValues[1] > (lineOffset[1] + 200))                     
+  else if(lineValue[1] < 1000)                 
   { 
+    while(lineValue[0] > 1000)
     motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
     ledRed(1); 
     delay(1000); 
@@ -314,19 +342,19 @@ void Crossroad()
   if(randy == 1) 
   {
     //zur Kreuzungsmitte fahren
-    while((countsLeft != 200) && (countsRight != 200))
+    while((wayLeft != 1) && (wayRight != 1))
     {
       EncoderUpdate();
       motors.setSpeeds(maxSpeed/2, maxSpeed/2);
     }
 
     //links drehen
-    turnAngle = 0;
-    TurnSensorUpdate();
-    while(turnAngle != 90)
+    rotationAngle = 0;
+    GyroUpdate();
+    while(rotationAngle != 90)
     {
     motors.setSpeeds(0, maxSpeed);  
-    TurnSensorUpdate();
+    GyroUpdate();
     }
 
     //geradeaus fahren
@@ -337,12 +365,12 @@ void Crossroad()
   else if(randy == 2) 
   {
     //rechts drehen
-    turnAngle = 0;
-    TurnSensorUpdate();
-    while(turnAngle != 90)
+    rotationAngle = 0;
+    GyroUpdate();
+    while(rotationAngle != 90)
     {
     motors.setSpeeds(maxSpeed, 0);  
-    TurnSensorUpdate();
+    GyroUpdate();
     }
 
     //geradeaus fahren
@@ -351,22 +379,13 @@ void Crossroad()
 }
 
 //Funktion Serielle Ausgabe
-/* 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, 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);
+    PSTR("Abstand: %3d %3d %3d %3d   Linie: %6d %6d %6d   Richtung: %3c   Weg: %6d %6d   Winkel: %6d"),
+    proxValue[0], proxValue[1], proxValue[2], proxValue[3], 
+    lineValue[0], lineValue[1], lineValue[2],
+    dir, wayLeft, wayRight, rotationAngle);
   Serial1.println(report);
 }
 
@@ -379,20 +398,16 @@ void loop()
   ledRed(0);
 
   //Abfragen der Sensordaten
-  lineSensors.read(lineValues);                                           
-  proxSensors.read();
-  proxValues[0] = proxSensors.countsLeftWithLeftLeds();
-  proxValues[1] = proxSensors.countsFrontWithLeftLeds();
-  proxValues[2] = proxSensors.countsFrontWithRightLeds();  
-  proxValues[3] = proxSensors.countsRightWithRightLeds();  
-  TurnSensorUpdate();
-  MoveSensorUpdate();
+  LineUpdate();                                         
+  ProxUpdate();
+  GyroUpdate();
+  CompassUpdate();
+  EncoderUpdate();
 
   //Funktionsauswahl
-/*   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();
+/*   if((compass.a.x - compassOffset) > 1000) CollisionDetection();         
+  else if((proxValue[0] || proxValue[1]) > 4) ObstacleAvoidance();
+  else if((lineValue[0] < 1000)) && (lineValue[1] < 1000)) && (lineValue[2] < 1000))) Crossroad();
   else TrackKeeper(); */
 
   //Ausgabe über Bluetoothverbindung