123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531 |
- #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: erstellt am 19.07.2017, zuletzt geändert am 21.02.2018
- //Projektbeschreibung: 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; //Motoren
- Zumo32U4ButtonA buttonA; //Taste A
- Zumo32U4Encoders encoders; //Motorencoder
- LSM303 compass; //Beschleunigungssensor x-Achse
- L3G gyro; //Drehbewegungssensor z-Achse
- int16_t lineValue[3]; //Liniensensoren
- uint16_t lineOffset[3]; //von links (0) nach rechts (2)
- uint8_t proxValue[4]; //Abstandssensoren v.l.(0)n.r.(3)
- int16_t encoderCounts[2]; //Anzahl der Umdrehungen
- int16_t driveRotation[2]; //von links (0) nach rechts (1)
- 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 (max. 400)
- char dir; //Fahrtrichtung, Ereignis
- char report[200]; //Ausgabe
- String btData; //Gelesene Daten aus Bluetooth
- /*-----------------------------------------------------------------*/
- //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 320 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
- void Abbiegen();
- #line 396 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
- void Spurhalten();
- #line 443 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
- void SerialOutput();
- #line 455 "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); //Fehlerabfrage
- 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");
- 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("Hindernisumfahren");
- 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] < 5 || proxValue[2] < 5)
- {
- //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");
- //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
- btData = Serial1.readString();
- //verfügbare Funktionen bei laufenden Manövern
- if(btData == "Abbiegen" || btData == "Hindernisumfahren" || btData == "Kollision")
- {
- maxSpeed = 100;
- if(moveRate > 1000 || proxValue[1] > 4 || proxValue[2] > 4 || lineValue[0] > 1000 ||
- lineValue[2] > 1000) motors.setSpeeds(0, 0);
- else Spurhalten();
- }
- //verfügbare Funktionen im Normalfall
- else
- {
- 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();
- }
|