improved functions

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -1,73 +1,78 @@
 //Verfassser: Felix Stange MET2016
-//Datum: 19.07.2017 erstellt, 02.08.2017 zuletzt geändert
+//Datum: 19.07.2017 erstellt, 03.08.2017 zuletzt geändert
 //Projekt: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe der 
-//Liniensensoren (3), ähnlich einer Spurhalteautomatik (dunkler Belag und hellen 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 Drehratensensor (L3G) 
-//genutzt. 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). 
+//genutzt. 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. 
 
 #include <Wire.h>
 #include <Zumo32U4.h>
 
-Zumo32U4ProximitySensors  proxSensors;
-Zumo32U4LineSensors       lineSensors;
+Zumo32U4ProximitySensors  proxSensors;  //Abstandssensoren
+Zumo32U4LineSensors       lineSensors;  //Liniensensoren
 Zumo32U4Motors            motors;
 Zumo32U4LCD               lcd;
 Zumo32U4ButtonA           buttonA;
-L3G                       gyro;
-LSM303                    compass;
+LSM303                    compass;      //Beschleunigungssensor x-Achse
+L3G                       gyro;         //Drehratensensor z-Achse
 
 uint16_t  lineValues[3];                //von links (0) nach rechts (2)
-uint16_t  lineReferences[3]; 
+uint16_t  lineOffset[3]; 
 
 uint8_t   proxValues[4];                //von links (0) nach rechts (3)
 
-int16_t   comValue;                     //Beschleunigungswerte auf x-Achse (längs des Zumo)
-
 uint32_t  turnAngle;                    //Drehwinkel in °
 int16_t   turnRate;
 int16_t   gyroOffset;
 uint16_t  gyroLastUpdate;
 
-int       maxSpeed = 200;                //Maximale Geschwindigkeit (möglich 400)
-char      dir;                           //Fahrtrichtung
-char      report[120];                   //Ausgabe über Serial
+int16_t   compassOffset;
+
+int       maxSpeed = 200;               //Maximale Geschwindigkeit (möglich 400)
+char      dir;                          //Fahrtrichtung, Ereignis
+char      report[120];                  //Ausgabe über Serial
 
-void LineSensorSetup()
+void LineSensorSetup()                                            //Setup Liniensensoren
 {
   lcd.clear();
-  lcd.print("Press A");
-  lcd.gotoXY(0, 1);
-  lcd.print("to calib");
-  buttonA.waitForButton();  
-  lcd.clear();
-  lcd.print("Calibrate");
+  lcd.print("Line cal");
+  ledYellow(1);
   delay(500);
   
-  for(uint16_t i = 0; i < 120; i++)                               //Kalibrierung
+  uint32_t total[3] = {0, 0, 0};  
+  for(uint8_t i = 0; i < 120; i++)                                //Kalibrierung
   {
     if (i > 30 && i <= 90) motors.setSpeeds(200, -200);
     else motors.setSpeeds(-200, 200);
-    lineSensors.calibrate();
+    lineSensors.read(lineOffset);  
+    total[0] += lineOffset[0];
+    total[1] += lineOffset[1];
+    total[2] += lineOffset[2];
+    lineSensors.calibrate();                                      
   }
+  ledYellow(0);
   motors.setSpeeds(0, 0);
+  lineOffset[0] = total[0] / 120;
+  lineOffset[1] = total[1] / 120;
+  lineOffset[2] = total[2] / 120;
 }
 
-void turnSensorSetup()
-{
-  gyro.init();
-  gyro.writeReg(L3G::CTRL1, 0b11111111);
-  gyro.writeReg(L3G::CTRL4, 0b00100000);
-  gyro.writeReg(L3G::CTRL5, 0b00000000);
-
+void TurnSensorSetup()                                            //Setup Drehratensensor
+{                                                                 
   lcd.clear();
   lcd.print("Gyro cal");
   ledYellow(1);
   delay(500);
+                                                                  //800Hz Ausgaberate
+  gyro.writeReg(L3G::CTRL1, 0b11111111);                          //Tiefpassfilter bei 100Hz
+  gyro.writeReg(L3G::CTRL4, 0b00100000);                          //2000dps Auflösung
+  gyro.writeReg(L3G::CTRL5, 0b00000000);                          //Hochpassfilter ausgeschaltet
 
-  int32_t total = 0;                                               //Kalibrierung
+  int32_t total = 0;                                              //Kalibrierung
   for (uint16_t i = 0; i < 1024; i++)
   {
     while(!gyro.readReg(L3G::STATUS_REG) & 0x08);
@@ -76,23 +81,42 @@ void turnSensorSetup()
   }
   ledYellow(0);
   gyroOffset = total / 1024;
+}
 
+void MoveSensorSetup()                                            //Setup Beschleunigungssensor
+{
   lcd.clear();
+  lcd.print("Acc cal");
+  ledYellow(1);
+  delay(500);
+
+  int32_t total = 0;                                              //Kalibrierung
+  for (uint16_t i = 0; i < 1024; i++)
+  {
+    compass.read();
+    total += compass.a.x;
+  }
+  ledYellow(0);
+  compassOffset = total / 1024;
 }
 
 void setup() 
 {
-  lineSensors.initThreeSensors();
-  lineSensors.read(lineReferences);  
+  lineSensors.initThreeSensors();                                   //Initialisierung der Sensoren
   proxSensors.initThreeSensors();
   Wire.begin();
-  gyro.init();
   compass.init();
   compass.enableDefault();  
+  gyro.init();
 
-  LineSensorSetup();
-
-  turnSensorSetup();
+  lcd.clear();                                                      //Kalibrierung der Sensoren
+  lcd.print("Press A");
+  lcd.gotoXY(0, 1);
+  lcd.print("to calib");
+  buttonA.waitForButton();  
+  LineSensorSetup();                                                
+  TurnSensorSetup();
+  MoveSensorSetup();
   gyroLastUpdate = micros();
   
   lcd.clear();
@@ -107,7 +131,7 @@ void setup()
   delay(500);
 }
 
-void CollisionDetection()
+void CollisionDetection()                                             //Funktion Kollisionserkennung
 {
   dir = 'X';
   motors.setSpeeds(0, 0);
@@ -116,20 +140,21 @@ void CollisionDetection()
   lcd.print("Impact");
   lcd.gotoXY(0, 1);
   lcd.print("detected");
-  while(1)                                                            //Fahrzeug stoppt; Reset erforderlich
-  {
 
-  }
+  motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
+  delay(1000); 
+
+  SerialOutput();
 }
 
-void ObstacleAvoidance()
+void ObstacleAvoidance()                                              //Funktion Hindernisumfahrung
 {
   dir = 'U';
 
   motors.setSpeeds(maxSpeed/2, maxSpeed);                             //Schritt 1: links bis über Mittellinie fahren
   turnAngle = 0;                                                      
-  turnSensorUpdate();                                                 
-  while(lineValues[2] < (lineReferences[2] -200))                
+  TurnSensorUpdate();                                                 
+  while(lineValues[2] < (lineOffset[2] -200))                
   {
     lineSensors.read(lineValues);
   }
@@ -137,7 +162,7 @@ void ObstacleAvoidance()
   motors.setSpeeds(maxSpeed, maxSpeed/2);                             //Schritt 2: rechts fahren bis Fahrzeug gerade steht
   while(turnAngle != 0)                                               //ohne dabei weitere Linien zu überfahren
   {                                                                   
-    turnSensorUpdate();
+    TurnSensorUpdate();
   }
 
   motors.setSpeeds(maxSpeed, maxSpeed);                               //Schritt 3: geradeaus fahren bis Hindernis passiert
@@ -152,8 +177,8 @@ void ObstacleAvoidance()
   
   motors.setSpeeds(maxSpeed/2, maxSpeed);                             //Schritt 4: rechts bis über Mittellinie fahren
   turnAngle = 0;                                                      
-  turnSensorUpdate();                                                 
-  while(lineValues[0] < (lineReferences[0] -200))                
+  TurnSensorUpdate();                                                 
+  while(lineValues[0] < (lineOffset[0] -200))                
   {
     lineSensors.read(lineValues);
   }
@@ -161,43 +186,47 @@ void ObstacleAvoidance()
   motors.setSpeeds(maxSpeed/2, maxSpeed);                             //Schritt 5: links fahren bis Fahrzeug gerade steht
   while(turnAngle != 0)                                               //ohne dabei weitere Linien zu überfahren
   {                                                                   
-    turnSensorUpdate();
+    TurnSensorUpdate();
   }  
+
+  SerialOutput();
 }
 
-void TrackKeeper()
+void TrackKeeper()                                                   //Funktion Spurhalten
 {
-  if(lineValues[0] < (lineReferences[0] - 200))
+  if(lineValues[0] < (lineOffset[0] - 200))                          //linke Linie erreicht, nach rechts fahren
   {
     motors.setSpeeds(maxSpeed, 0);
     ledYellow(1);
     delay(200);    
     dir = 'R';
   }
-  else if(lineValues[2] < (lineReferences[2] - 200))
+  else if(lineValues[2] < (lineOffset[2] - 200))                     //rechte Linie erreicht, nach links fahren
   {
     motors.setSpeeds(0, maxSpeed);  
     ledYellow(1); 
     delay(200); 
     dir = 'L';     
   }
-  else if(lineValues[1] < (lineReferences[1] - 100))
-  {
+  else if(lineValues[1] < (lineOffset[1] - 100))                     //Linie überfahren, zurücksetzen
+  { 
     motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
     ledRed(1); 
     delay(1000); 
     dir = 'B';     
   }  
-  else
+  else                                                                
   {
     motors.setSpeeds(maxSpeed, maxSpeed);    
     ledGreen(1); 
     delay(100);
     dir = 'F';     
   }
+
+  SerialOutput();
 }
 
-void turnSensorUpdate()
+void TurnSensorUpdate()                                                 //Funktion Drehratensensor
 {
   gyro.read();
   turnRate = gyro.g.z - gyroOffset;
@@ -208,13 +237,13 @@ void turnSensorUpdate()
   turnAngle += (int64_t)d * 14680064 / 17578125;
 }
 
-void SerialOutput()
+void SerialOutput()                                                     //Funktion Serielle Ausgabe
 {
   snprintf_P(report, sizeof(report),
-    PSTR("Abstand: %3d %3d   Linien: %6d %6d %6d   Richtung: %3c   Beschleunigung: %6d"),
+    PSTR("Abstand: %3d %3d   Linien: %6d %6d %6d   Richtung: %3c   Beschleunigung: %6d   Drehwinkel: %6d"),
     proxValues[0], proxValues[1], proxValues[2], proxValues[3], 
     lineValues[0], lineValues[1], lineValues[2],
-    dir, comValue);
+    dir, compass.a.x, gyro.g.z);
   Serial.println(report);
 //  if(Serial1.available()) Serial1.println(report);
 }
@@ -226,18 +255,16 @@ void loop()
   ledRed(0);
   lcd.clear();
 
-  lineSensors.read(lineValues);
+  lineSensors.read(lineValues);                                           //Abfragen der Sensordaten
   proxSensors.read();
   proxValues[0] = proxSensors.countsLeftWithLeftLeds();
   proxValues[1] = proxSensors.countsFrontWithLeftLeds();
   proxValues[2] = proxSensors.countsFrontWithRightLeds();  
   proxValues[3] = proxSensors.countsRightWithRightLeds();  
   compass.read();
-  comValue = compass.a.x;
+  gyro.read();
 
-  if(comValue > 16000) CollisionDetection();
+  if((compass.a.x - compassOffset) > 16000) CollisionDetection();         //Funktionsauswahl
   else if((proxValues[0] || proxValues[1]) > 4) ObstacleAvoidance();
   else TrackKeeper();
-
-  SerialOutput();
 }