|
@@ -0,0 +1,529 @@
|
|
|
+#include <Arduino.h>
|
|
|
+#line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+#line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+//Verfassser: Felix Stange 4056379 MET2016
|
|
|
+//Datum: 19.07.2017 erstellt, 01.02.2018 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).
|
|
|
+//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)
|
|
|
+//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 Bluetoothnutzung nicht verwendet werden.
|
|
|
+
|
|
|
+#include <Wire.h>
|
|
|
+#include <Zumo32U4.h>
|
|
|
+
|
|
|
+Zumo32U4ProximitySensors proxSensors; //Abstandssensoren
|
|
|
+Zumo32U4LineSensors lineSensors; //Liniensensoren
|
|
|
+Zumo32U4Motors motors;
|
|
|
+Zumo32U4ButtonA buttonA;
|
|
|
+Zumo32U4Encoders encoders; //Motorencoder
|
|
|
+LSM303 compass; //Beschleunigungssensor x-Achse
|
|
|
+L3G gyro; //Drehbewegungssensor z-Achse
|
|
|
+
|
|
|
+int16_t lineValue[3]; //von links (0) nach rechts (2)
|
|
|
+uint16_t lineOffset[3];
|
|
|
+
|
|
|
+uint8_t proxValue[4]; //von links (0) nach rechts (3)
|
|
|
+
|
|
|
+int16_t encoderCounts[2]; //von links (0) nach rechts (1)
|
|
|
+int16_t driveRotation[2];
|
|
|
+
|
|
|
+int32_t rotationAngle = 0; //Drehwinkel
|
|
|
+int32_t turnAngle = 0;
|
|
|
+int16_t turnRate;
|
|
|
+int16_t gyroOffset;
|
|
|
+uint16_t gyroLastUpdate;
|
|
|
+
|
|
|
+int32_t moveDisplay = 0; //Beschleunigung
|
|
|
+int32_t moveDistance = 0;
|
|
|
+int16_t moveRate;
|
|
|
+int16_t compassOffset;
|
|
|
+uint16_t compassLastUpdate;
|
|
|
+
|
|
|
+uint16_t LastUpdate = 0; //Systemzeit
|
|
|
+uint16_t CycleTime = 0; //Zykluszeit
|
|
|
+int maxSpeed = 200; //Maximale Geschwindigkeit (möglich 400)
|
|
|
+char dir; //Fahrtrichtung, Ereignis
|
|
|
+char report[200]; //Ausgabe
|
|
|
+int warning = 0; //1 zeigt Überhol-/Abbiegevorgang an
|
|
|
+
|
|
|
+/*-----------------------------------------------------------------*/
|
|
|
+
|
|
|
+//Setup Liniensensoren
|
|
|
+#line 54 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void LineSetup();
|
|
|
+#line 79 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void ProxSetup();
|
|
|
+#line 85 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void GyroSetup();
|
|
|
+#line 109 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void CompassSetup();
|
|
|
+#line 130 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void setup();
|
|
|
+#line 157 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void TimeUpdate();
|
|
|
+#line 165 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void ProxUpdate();
|
|
|
+#line 175 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void LineUpdate();
|
|
|
+#line 185 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void GyroUpdate();
|
|
|
+#line 198 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void CompassUpdate();
|
|
|
+#line 211 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void EncoderUpdate();
|
|
|
+#line 222 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void Kollisionserkennung();
|
|
|
+#line 238 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void Hindernisumfahren();
|
|
|
+#line 321 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void Abbiegen();
|
|
|
+#line 398 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void Spurhalten();
|
|
|
+#line 445 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void SerialOutput();
|
|
|
+#line 457 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+void loop();
|
|
|
+#line 54 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
|
+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(maxSpeed, -maxSpeed);
|
|
|
+ else motors.setSpeeds(-maxSpeed, maxSpeed);
|
|
|
+ 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;
|
|
|
+}
|
|
|
+
|
|
|
+//Setup Abstandssensoren
|
|
|
+void ProxSetup()
|
|
|
+{
|
|
|
+ proxSensors.initThreeSensors();
|
|
|
+}
|
|
|
+
|
|
|
+//Setup Drehbewegungssensor
|
|
|
+void GyroSetup()
|
|
|
+{
|
|
|
+ ledYellow(1);
|
|
|
+ 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
|
|
|
+
|
|
|
+ //Kalibrierung
|
|
|
+ int32_t total = 0;
|
|
|
+ for (uint16_t i = 0; i < 1024; i++)
|
|
|
+ {
|
|
|
+ while(!gyro.readReg(L3G::STATUS_REG) & 0x08);
|
|
|
+ gyro.read();
|
|
|
+ total += gyro.g.z;
|
|
|
+ }
|
|
|
+ gyroOffset = total / 1024;
|
|
|
+
|
|
|
+ gyroLastUpdate = micros();
|
|
|
+ ledYellow(0);
|
|
|
+}
|
|
|
+
|
|
|
+//Setup Beschleunigungssensor
|
|
|
+void CompassSetup()
|
|
|
+{
|
|
|
+ ledYellow(1);
|
|
|
+ compass.init();
|
|
|
+ compass.enableDefault();
|
|
|
+
|
|
|
+ //Kalibrierung
|
|
|
+ int32_t total = 0;
|
|
|
+ for (uint16_t i = 0; i < 1024; i++)
|
|
|
+ {
|
|
|
+ compass.read();
|
|
|
+ total += compass.a.x;
|
|
|
+ }
|
|
|
+ compassOffset = total / 1024;
|
|
|
+
|
|
|
+ compassLastUpdate = micros();
|
|
|
+ ledYellow(0);
|
|
|
+}
|
|
|
+
|
|
|
+/*-----------------------------------------------------------------*/
|
|
|
+
|
|
|
+void setup()
|
|
|
+{
|
|
|
+ //Initialisierung der Bluetoothverbindung
|
|
|
+ Serial1.begin(9600);
|
|
|
+ if(Serial1.available()) Serial1.println("Verbindung hergestellt");
|
|
|
+
|
|
|
+ //Initialisierung und Kalibrierung der Sensoren
|
|
|
+ Serial1.println("Sensorkalibrierung");
|
|
|
+ Wire.begin();
|
|
|
+ delay(500);
|
|
|
+ LastUpdate = micros();
|
|
|
+ ProxSetup();
|
|
|
+ LineSetup();
|
|
|
+ GyroSetup();
|
|
|
+ CompassSetup();
|
|
|
+
|
|
|
+ //Zumo bereit zu starten
|
|
|
+ Serial1.println("Zumo bereit, starte mit A");
|
|
|
+ ledGreen(1);
|
|
|
+ buttonA.waitForButton();
|
|
|
+ randomSeed(millis());
|
|
|
+ delay(500);
|
|
|
+}
|
|
|
+
|
|
|
+/*-----------------------------------------------------------------*/
|
|
|
+
|
|
|
+//Update Durchlaufzeit
|
|
|
+void TimeUpdate()
|
|
|
+{
|
|
|
+ uint16_t m = micros();
|
|
|
+ CycleTime = m - LastUpdate;
|
|
|
+ LastUpdate = m;
|
|
|
+}
|
|
|
+
|
|
|
+//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()
|
|
|
+{
|
|
|
+ uint16_t lineRaw[3];
|
|
|
+ lineSensors.read(lineRaw);
|
|
|
+ lineValue[0] = lineRaw[0] - lineOffset[0];
|
|
|
+ lineValue[1] = lineRaw[1] - lineOffset[1];
|
|
|
+ lineValue[2] = lineRaw[2] - lineOffset[2];
|
|
|
+}
|
|
|
+
|
|
|
+//Update Drehbewegungssensor
|
|
|
+void GyroUpdate()
|
|
|
+{
|
|
|
+ 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;
|
|
|
+ rotationAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
|
|
|
+}
|
|
|
+
|
|
|
+// Update Beschleunigungssensor
|
|
|
+void CompassUpdate()
|
|
|
+{
|
|
|
+ compass.read();
|
|
|
+ moveRate = compass.a.x - compassOffset;
|
|
|
+ uint16_t m = micros();
|
|
|
+ uint16_t dt = m - compassLastUpdate;
|
|
|
+ compassLastUpdate = m;
|
|
|
+ int32_t d = (int32_t)moveRate * dt;
|
|
|
+ moveDistance += (int64_t)d * dt >> 14;
|
|
|
+ moveDisplay = moveDistance * 1000 / 9,81;
|
|
|
+}
|
|
|
+
|
|
|
+//Update Encoder
|
|
|
+void EncoderUpdate()
|
|
|
+{
|
|
|
+ encoderCounts[0] = encoders.getCountsLeft();
|
|
|
+ driveRotation[0] = encoderCounts[0] / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
|
|
+ encoderCounts[1] = encoders.getCountsRight();
|
|
|
+ driveRotation[1] = encoderCounts[1] / 910;
|
|
|
+}
|
|
|
+
|
|
|
+/*-----------------------------------------------------------------*/
|
|
|
+
|
|
|
+ //Funktion Kollisionserkennung
|
|
|
+void Kollisionserkennung()
|
|
|
+{
|
|
|
+ dir = 'X';
|
|
|
+ Serial1.println("Kollision erkannt");
|
|
|
+ Serial1.println("1");
|
|
|
+ ledRed(1);
|
|
|
+ motors.setSpeeds(0, 0);
|
|
|
+ delay(500);
|
|
|
+ while(lineValue[0] < 300 && lineValue[2] < 300)
|
|
|
+ {
|
|
|
+ motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
+ }
|
|
|
+ delay(500);
|
|
|
+}
|
|
|
+
|
|
|
+//Funktion Hindernisumfahrung
|
|
|
+void Hindernisumfahren()
|
|
|
+{
|
|
|
+ dir = 'U';
|
|
|
+ Serial1.println("Hindernis umfahren");
|
|
|
+ Serial1.println("1");
|
|
|
+ ledYellow(1);
|
|
|
+
|
|
|
+ //Schritt 1: Spurwechsel links
|
|
|
+ //links drehen
|
|
|
+ rotationAngle = 0;
|
|
|
+ GyroUpdate();
|
|
|
+ while(rotationAngle < 45)
|
|
|
+ {
|
|
|
+ GyroUpdate();
|
|
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
+ }
|
|
|
+ //geradeaus über Mittellinie fahren
|
|
|
+ LineUpdate();
|
|
|
+ while(lineValue[2] < 300)
|
|
|
+ {
|
|
|
+ LineUpdate();
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ }
|
|
|
+ //rechts drehen
|
|
|
+ GyroUpdate();
|
|
|
+ while(rotationAngle > 0)
|
|
|
+ {
|
|
|
+ GyroUpdate();
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
+ }
|
|
|
+ //geradeaus fahren
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+
|
|
|
+ //Gegenverkehr beachten
|
|
|
+ proxSensors.read();
|
|
|
+ proxValue[1] = proxSensors.countsFrontWithLeftLeds();
|
|
|
+ proxValue[2] = proxSensors.countsFrontWithRightLeds();
|
|
|
+ if(proxValue[1] < 4 || proxValue[2] < 4)
|
|
|
+ {
|
|
|
+ //Schritt 2: Hindernis passieren
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ delay(1000);
|
|
|
+ proxSensors.read();
|
|
|
+ proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
|
+ while(proxValue[3] > 4)
|
|
|
+ {
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ proxSensors.read();
|
|
|
+ proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
|
+ Spurhalten();
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ //Schritt 3: Spurwechsel rechts
|
|
|
+ //rechts drehen
|
|
|
+ rotationAngle = 0;
|
|
|
+ GyroUpdate();
|
|
|
+ while(rotationAngle > -45)
|
|
|
+ {
|
|
|
+ GyroUpdate();
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
+ }
|
|
|
+ //geradeaus über Mittellinie fahren
|
|
|
+ LineUpdate();
|
|
|
+ while(lineValue[0] < 300)
|
|
|
+ {
|
|
|
+ LineUpdate();
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ }
|
|
|
+ //links drehen
|
|
|
+ GyroUpdate();
|
|
|
+ while(rotationAngle < 0)
|
|
|
+ {
|
|
|
+ GyroUpdate();
|
|
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
+ }
|
|
|
+ //geradeaus fahren
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+
|
|
|
+ Serial1.println("Umfahren beendet");
|
|
|
+}
|
|
|
+
|
|
|
+//Funktion Abbiegen
|
|
|
+void Abbiegen()
|
|
|
+{
|
|
|
+ dir = 'A';
|
|
|
+ ledYellow(1);
|
|
|
+ Serial1.println("Abbiegen");
|
|
|
+ Serial1.println("1");
|
|
|
+
|
|
|
+ //Kodierung analysieren
|
|
|
+ bool leftCode; //links => 1
|
|
|
+ bool rightCode; //rechts => 2
|
|
|
+ if(lineValue[0] > 1000) leftCode = true;
|
|
|
+ else leftCode = false;
|
|
|
+ if(lineValue[2] > 1000) rightCode = true;
|
|
|
+ else rightCode = false;
|
|
|
+
|
|
|
+ //Zufallszahl erzeugen
|
|
|
+ uint8_t randy; //geradeaus => 0
|
|
|
+ if(leftCode == true && rightCode == true) randy = random(1, 3); //1, 2
|
|
|
+ else if(leftCode == true && rightCode == false) randy = random(0, 2); //0, 1
|
|
|
+ else if(leftCode == false && rightCode == true)
|
|
|
+ {
|
|
|
+ randy = random(3); //0, (1), 2
|
|
|
+ while(randy == 1) randy = random(3); //!1 => 0, 2
|
|
|
+ }
|
|
|
+
|
|
|
+ //links Abbiegen
|
|
|
+ if(randy == 1 && leftCode == true)
|
|
|
+ {
|
|
|
+ //zur Kreuzungsmitte fahren
|
|
|
+ driveRotation[0] = 0;
|
|
|
+ driveRotation[1] = 0;
|
|
|
+ while(driveRotation[0] != 1 && driveRotation[1] != 1 && lineValue[0] < 300 && lineValue[2] < 300)
|
|
|
+ {
|
|
|
+ EncoderUpdate();
|
|
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
+ }
|
|
|
+ //links drehen
|
|
|
+ rotationAngle = 0;
|
|
|
+ GyroUpdate();
|
|
|
+ while(rotationAngle != 90)
|
|
|
+ {
|
|
|
+ GyroUpdate();
|
|
|
+ if(lineValue[0] > 300 && lineValue[0] < 500) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
+ else if(lineValue[1] > 300 && lineValue[1] < 500) motors.setSpeeds(-maxSpeed/4, -maxSpeed/4);
|
|
|
+ else if(lineValue[2] > 300 && lineValue[2] < 500) motors.setSpeeds(0, maxSpeed/2);
|
|
|
+ else motors.setSpeeds(maxSpeed/4, maxSpeed/2);
|
|
|
+ }
|
|
|
+ //geradeaus fahren
|
|
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
+ }
|
|
|
+
|
|
|
+ //rechts Abbiegen
|
|
|
+ else if(randy == 2 && rightCode == true)
|
|
|
+ {
|
|
|
+ //rechts drehen
|
|
|
+ rotationAngle = 0;
|
|
|
+ GyroUpdate();
|
|
|
+ while(rotationAngle != -90)
|
|
|
+ {
|
|
|
+ GyroUpdate();
|
|
|
+ if(lineValue[0] > 300 && lineValue[0] < 500) motors.setSpeeds(maxSpeed/2, 0);
|
|
|
+ else if(lineValue[1] > 300 && lineValue[1] < 500) motors.setSpeeds(-maxSpeed/4, -maxSpeed/4);
|
|
|
+ else if(lineValue[2] > 300 && lineValue[2] < 500) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
+ else motors.setSpeeds(maxSpeed/2, maxSpeed/4);
|
|
|
+ }
|
|
|
+ //geradeaus fahren
|
|
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
+ }
|
|
|
+
|
|
|
+ //nicht Abbiegen, geradeaus fahren
|
|
|
+ else motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
+ delay(500);
|
|
|
+
|
|
|
+ Serial1.println("Abbiegen beendet");
|
|
|
+}
|
|
|
+
|
|
|
+//Funktion Spurhalten
|
|
|
+void Spurhalten()
|
|
|
+{
|
|
|
+ //linke Linie erreicht, nach rechts fahren
|
|
|
+ if(lineValue[0] > 300 && lineValue[0] < 500)
|
|
|
+ {
|
|
|
+ dir = 'R';
|
|
|
+ ledYellow(1);
|
|
|
+ Serial1.println("Spur nach rechts korrigieren");
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
+ delay(100);
|
|
|
+ }
|
|
|
+
|
|
|
+ //rechte Linie erreicht, nach links fahren
|
|
|
+ else if(lineValue[2] > 300 && lineValue[2] < 500)
|
|
|
+ {
|
|
|
+ dir = 'L';
|
|
|
+ ledYellow(1);
|
|
|
+ Serial1.println("Spur nach links korrigieren");
|
|
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
+ delay(100);
|
|
|
+ }
|
|
|
+
|
|
|
+ //Linie überfahren, zurücksetzen
|
|
|
+ else if(lineValue[1] > 300 && lineValue[1] < 500)
|
|
|
+ {
|
|
|
+ dir = 'B';
|
|
|
+ ledRed(1);
|
|
|
+ Serial1.println("Spur verlassen");
|
|
|
+ Serial1.println("1");
|
|
|
+ while(lineValue[0] < 300 && lineValue[2] < 300) //Zumo fährt solange rückwärst bis er wieder auf der Spur steht
|
|
|
+ {
|
|
|
+ motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
+ }
|
|
|
+ delay(500);
|
|
|
+ }
|
|
|
+
|
|
|
+ //normale Fahrt
|
|
|
+ else
|
|
|
+ {
|
|
|
+ dir = 'F';
|
|
|
+ ledGreen(1);
|
|
|
+ Serial1.println("Spur folgen");
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+//Funktion Serielle Ausgabe
|
|
|
+void SerialOutput()
|
|
|
+{
|
|
|
+ snprintf_P(report, sizeof(report),
|
|
|
+ PSTR("Abstand: %d %d %d %d Linie: %d %d %d Weg: %d %d Winkel: %d Zeit: %d"),
|
|
|
+ proxValue[0], proxValue[1], proxValue[2], proxValue[3],
|
|
|
+ lineValue[0], lineValue[1], lineValue[2],
|
|
|
+ driveRotation[0], driveRotation[1], rotationAngle, CycleTime);
|
|
|
+ Serial1.println(report);
|
|
|
+}
|
|
|
+
|
|
|
+/*-----------------------------------------------------------------*/
|
|
|
+
|
|
|
+void loop()
|
|
|
+{
|
|
|
+ ledGreen(0);
|
|
|
+ ledYellow(0);
|
|
|
+ ledRed(0);
|
|
|
+
|
|
|
+ //Abfragen der Sensordaten
|
|
|
+ TimeUpdate();
|
|
|
+ LineUpdate();
|
|
|
+ ProxUpdate();
|
|
|
+ GyroUpdate();
|
|
|
+ CompassUpdate();
|
|
|
+ EncoderUpdate();
|
|
|
+
|
|
|
+ //Funktionsauswahl
|
|
|
+ warning = Serial1.read();
|
|
|
+ if(warning == 1) //verfügbare Funktionen bei laufenden Manövern
|
|
|
+ {
|
|
|
+ maxSpeed = 100;
|
|
|
+ if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || lineValue[0] > 1000 ||
|
|
|
+ lineValue[2] > 1000) motors.setSpeeds(0, 0);
|
|
|
+ else Spurhalten();
|
|
|
+ }
|
|
|
+ else //verfügbare Funktionen im Normalfall
|
|
|
+ {
|
|
|
+ if(moveRate > 1000) Kollisionserkennung();
|
|
|
+ else if(proxValue[1] == 6 || proxValue[2] == 6) motors.setSpeeds(0, 0);
|
|
|
+ else if(proxValue[1] == 5 || proxValue[2] == 5) Hindernisumfahren();
|
|
|
+ else if(lineValue[0] > 1000 || lineValue[2] > 1000) Abbiegen();
|
|
|
+ else Spurhalten();
|
|
|
+ }
|
|
|
+
|
|
|
+ //Ausgabe über Bluetoothverbindung
|
|
|
+ SerialOutput();
|
|
|
+}
|