|
@@ -1,32 +1,41 @@
|
|
//Verfassser: Felix Stange MET2016
|
|
//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
|
|
//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
|
|
//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).
|
|
//erfolgt seriell (SERIAL für USB, SERIAL1 für Bluetooth).
|
|
|
|
|
|
#include <Wire.h>
|
|
#include <Wire.h>
|
|
#include <Zumo32U4.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.clear();
|
|
lcd.print("Press A");
|
|
lcd.print("Press A");
|
|
@@ -37,7 +46,7 @@ void LineSensorCalibration()
|
|
lcd.print("Calibrate");
|
|
lcd.print("Calibrate");
|
|
delay(500);
|
|
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);
|
|
if (i > 30 && i <= 90) motors.setSpeeds(200, -200);
|
|
else motors.setSpeeds(-200, 200);
|
|
else motors.setSpeeds(-200, 200);
|
|
@@ -46,16 +55,45 @@ void LineSensorCalibration()
|
|
motors.setSpeeds(0, 0);
|
|
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()
|
|
void setup()
|
|
{
|
|
{
|
|
- lineSensors.initFiveSensors();
|
|
|
|
|
|
+ lineSensors.initThreeSensors();
|
|
lineSensors.read(lineReferences);
|
|
lineSensors.read(lineReferences);
|
|
- proxSensors.initFrontSensor();
|
|
|
|
|
|
+ proxSensors.initThreeSensors();
|
|
Wire.begin();
|
|
Wire.begin();
|
|
|
|
+ gyro.init();
|
|
compass.init();
|
|
compass.init();
|
|
compass.enableDefault();
|
|
compass.enableDefault();
|
|
|
|
|
|
- LineSensorCalibration();
|
|
|
|
|
|
+ LineSensorSetup();
|
|
|
|
+
|
|
|
|
+ turnSensorSetup();
|
|
|
|
+ gyroLastUpdate = micros();
|
|
|
|
|
|
lcd.clear();
|
|
lcd.clear();
|
|
lcd.print("Press A");
|
|
lcd.print("Press A");
|
|
@@ -69,12 +107,113 @@ void setup()
|
|
delay(500);
|
|
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()
|
|
void SerialOutput()
|
|
{
|
|
{
|
|
snprintf_P(report, sizeof(report),
|
|
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);
|
|
dir, comValue);
|
|
Serial.println(report);
|
|
Serial.println(report);
|
|
// if(Serial1.available()) Serial1.println(report);
|
|
// if(Serial1.available()) Serial1.println(report);
|
|
@@ -89,11 +228,16 @@ void loop()
|
|
|
|
|
|
lineSensors.read(lineValues);
|
|
lineSensors.read(lineValues);
|
|
proxSensors.read();
|
|
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();
|
|
compass.read();
|
|
comValue = compass.a.x;
|
|
comValue = compass.a.x;
|
|
- dir = 'x';
|
|
|
|
|
|
+
|
|
|
|
+ if(comValue > 16000) CollisionDetection();
|
|
|
|
+ else if((proxValues[0] || proxValues[1]) > 4) ObstacleAvoidance();
|
|
|
|
+ else TrackKeeper();
|
|
|
|
|
|
SerialOutput();
|
|
SerialOutput();
|
|
}
|
|
}
|