functions added for collsisiondetection and obstacleavoidance

Zumo32U4/Hauptprojekt/Hauptprojekt.ino

@@ -1,32 +1,41 @@
 //Verfassser: Felix Stange MET2016
-//Datum: 20.07.2017
+//Datum: 19.07.2017 erstellt, 02.08.2017 zuletzt geändert
 //Projekt: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe der 
-//Liniensensoren (5), ähnlich einer Spurhalteautomatik (dunkler Belag und hellen Streifen). 
+//Liniensensoren (3), ähnlich einer Spurhalteautomatik (dunkler Belag und hellen Streifen). 
 //Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es 
-//dennoch zu einer Kollision, wird diese durch die Beschleunigungssensoren erkannt. 
-//Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die Kommunikation 
+//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). 
 
 #include <Wire.h>
 #include <Zumo32U4.h>
 
-Zumo32U4ProximitySensors proxSensors;
-Zumo32U4LineSensors lineSensors;
-Zumo32U4Motors motors;
-Zumo32U4LCD lcd;
-Zumo32U4ButtonA buttonA;
-LSM303 compass;
+Zumo32U4ProximitySensors  proxSensors;
+Zumo32U4LineSensors       lineSensors;
+Zumo32U4Motors            motors;
+Zumo32U4LCD               lcd;
+Zumo32U4ButtonA           buttonA;
+L3G                       gyro;
+LSM303                    compass;
 
-uint16_t lineValues[5];               //von links (0) nach rechts (5)
-uint16_t lineReferences[5]; 
-uint8_t proxValues[2];                //Frontempfänger mit Werten von linker und rechter LED
-int16_t comValue;                     //Beschleunigungswerte auf x-Achse (längs des Zumo)
+uint16_t  lineValues[3];                //von links (0) nach rechts (2)
+uint16_t  lineReferences[3]; 
 
-int maxSpeed = 200;                   //Maximale Geschwindigkeit (möglich 400)
-char dir;                             //Fahrtrichtung
-char report[120];                     //Ausgabe über Serial
+uint8_t   proxValues[4];                //von links (0) nach rechts (3)
 
-void LineSensorCalibration()
+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
+
+void LineSensorSetup()
 {
   lcd.clear();
   lcd.print("Press A");
@@ -37,7 +46,7 @@ void LineSensorCalibration()
   lcd.print("Calibrate");
   delay(500);
   
-  for(uint16_t i = 0; i < 120; i++)
+  for(uint16_t i = 0; i < 120; i++)                               //Kalibrierung
   {
     if (i > 30 && i <= 90) motors.setSpeeds(200, -200);
     else motors.setSpeeds(-200, 200);
@@ -46,16 +55,45 @@ void LineSensorCalibration()
   motors.setSpeeds(0, 0);
 }
 
+void turnSensorSetup()
+{
+  gyro.init();
+  gyro.writeReg(L3G::CTRL1, 0b11111111);
+  gyro.writeReg(L3G::CTRL4, 0b00100000);
+  gyro.writeReg(L3G::CTRL5, 0b00000000);
+
+  lcd.clear();
+  lcd.print("Gyro cal");
+  ledYellow(1);
+  delay(500);
+
+  int32_t total = 0;                                               //Kalibrierung
+  for (uint16_t i = 0; i < 1024; i++)
+  {
+    while(!gyro.readReg(L3G::STATUS_REG) & 0x08);
+    gyro.read();
+    total += gyro.g.z;
+  }
+  ledYellow(0);
+  gyroOffset = total / 1024;
+
+  lcd.clear();
+}
+
 void setup() 
 {
-  lineSensors.initFiveSensors();
+  lineSensors.initThreeSensors();
   lineSensors.read(lineReferences);  
-  proxSensors.initFrontSensor();
+  proxSensors.initThreeSensors();
   Wire.begin();
+  gyro.init();
   compass.init();
   compass.enableDefault();  
 
-  LineSensorCalibration();
+  LineSensorSetup();
+
+  turnSensorSetup();
+  gyroLastUpdate = micros();
   
   lcd.clear();
   lcd.print("Press A");
@@ -69,12 +107,113 @@ void setup()
   delay(500);
 }
 
+void CollisionDetection()
+{
+  dir = 'X';
+  motors.setSpeeds(0, 0);
+  ledRed(1);
+  lcd.clear();
+  lcd.print("Impact");
+  lcd.gotoXY(0, 1);
+  lcd.print("detected");
+  while(1)                                                            //Fahrzeug stoppt; Reset erforderlich
+  {
+
+  }
+}
+
+void ObstacleAvoidance()
+{
+  dir = 'U';
+
+  motors.setSpeeds(maxSpeed/2, maxSpeed);                             //Schritt 1: links bis über Mittellinie fahren
+  turnAngle = 0;                                                      
+  turnSensorUpdate();                                                 
+  while(lineValues[2] < (lineReferences[2] -200))                
+  {
+    lineSensors.read(lineValues);
+  }
+
+  motors.setSpeeds(maxSpeed, maxSpeed/2);                             //Schritt 2: rechts fahren bis Fahrzeug gerade steht
+  while(turnAngle != 0)                                               //ohne dabei weitere Linien zu überfahren
+  {                                                                   
+    turnSensorUpdate();
+  }
+
+  motors.setSpeeds(maxSpeed, maxSpeed);                               //Schritt 3: geradeaus fahren bis Hindernis passiert
+  delay(1000);
+  proxSensors.read();
+  proxValues[3] = proxSensors.countsRightWithRightLeds();  
+  while(proxValues[3] > 4)
+  {
+    proxSensors.read();
+    proxValues[3] = proxSensors.countsRightWithRightLeds();  
+  }
+  
+  motors.setSpeeds(maxSpeed/2, maxSpeed);                             //Schritt 4: rechts bis über Mittellinie fahren
+  turnAngle = 0;                                                      
+  turnSensorUpdate();                                                 
+  while(lineValues[0] < (lineReferences[0] -200))                
+  {
+    lineSensors.read(lineValues);
+  }
+
+  motors.setSpeeds(maxSpeed/2, maxSpeed);                             //Schritt 5: links fahren bis Fahrzeug gerade steht
+  while(turnAngle != 0)                                               //ohne dabei weitere Linien zu überfahren
+  {                                                                   
+    turnSensorUpdate();
+  }  
+}
+
+void TrackKeeper()
+{
+  if(lineValues[0] < (lineReferences[0] - 200))
+  {
+    motors.setSpeeds(maxSpeed, 0);
+    ledYellow(1);
+    delay(200);    
+    dir = 'R';
+  }
+  else if(lineValues[2] < (lineReferences[2] - 200))
+  {
+    motors.setSpeeds(0, maxSpeed);  
+    ledYellow(1); 
+    delay(200); 
+    dir = 'L';     
+  }
+  else if(lineValues[1] < (lineReferences[1] - 100))
+  {
+    motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
+    ledRed(1); 
+    delay(1000); 
+    dir = 'B';     
+  }  
+  else
+  {
+    motors.setSpeeds(maxSpeed, maxSpeed);    
+    ledGreen(1); 
+    delay(100);
+    dir = 'F';     
+  }
+}
+
+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;
+}
+
 void SerialOutput()
 {
   snprintf_P(report, sizeof(report),
-    PSTR("Abstand: %3d %3d   Linien: %6d %6d %6d %6d %6d   Richtung: %3c   Beschleunigung: %6d"),
-    proxValues[0], proxValues[1], 
-    lineValues[0], lineValues[1], lineValues[2], lineValues[3], lineValues[4], 
+    PSTR("Abstand: %3d %3d   Linien: %6d %6d %6d   Richtung: %3c   Beschleunigung: %6d"),
+    proxValues[0], proxValues[1], proxValues[2], proxValues[3], 
+    lineValues[0], lineValues[1], lineValues[2],
     dir, comValue);
   Serial.println(report);
 //  if(Serial1.available()) Serial1.println(report);
@@ -89,11 +228,16 @@ void loop()
 
   lineSensors.read(lineValues);
   proxSensors.read();
-  proxValues[0] = proxSensors.countsFrontWithLeftLeds();
-  proxValues[1] = proxSensors.countsFrontWithRightLeds();  
+  proxValues[0] = proxSensors.countsLeftWithLeftLeds();
+  proxValues[1] = proxSensors.countsFrontWithLeftLeds();
+  proxValues[2] = proxSensors.countsFrontWithRightLeds();  
+  proxValues[3] = proxSensors.countsRightWithRightLeds();  
   compass.read();
   comValue = compass.a.x;
-  dir = 'x';
+
+  if(comValue > 16000) CollisionDetection();
+  else if((proxValues[0] || proxValues[1]) > 4) ObstacleAvoidance();
+  else TrackKeeper();
 
   SerialOutput();
 }