Arduino: 1.8.19 (Windows 10), Board: "ESP32 Dev Module, Disabled, Default 4MB with spiffs (1.2MB APP/1.5MB SPIFFS), 240MHz (WiFi/BT), QIO, 80MHz, 4MB (32Mb), 921600, Core 1, Core 1, None"
C:\Users\muffd\OneDrive\Dokumente\Arduino\libraries\ZumoShield\PololuBuzzer.cpp:3:10: fatal error: avr/interrupt.h: No such file or directory
#include <avr/interrupt.h>
^~~~~~~~~~~~~~~~~
compilation terminated.
exit status 1
Error compiling for board ESP32 Dev Module.
my Code:
// Einbinden der Bibliothek für das Bluetooth-Modul
#include <SoftwareSerial.h>
// Einbinden der NewPing-Bibliothek (Zur Verbesserung der Arbeit des Sensors)
#include <NewPing.h>
// Einbinden der Zumo-Motor-Bibliothek (zur einfachen Verwendung der Motoren)
#include <ZumoMotors.h>
// Einbinden der Bibliothek zur Benutzung des Buttons des Zumo-Bausatzes
#include <Pushbutton.h>
// Einbinden der Bibliothek zur Nutzung des I2C-Bus
#include <Wire.h>
// Einbinden der Bibliothek zur Nutzung des Magnetometers
#include <LSM303.h>
// Definieren der Pinbelegung mittels define-Befehlen
#define TasterPin 2 // Tasterabfrage für den Notstopp an Pin 2
#define Empfangspin 3 // = rx auf Modul an Pin 3
#define Sendepin 4 // = tx auf Modul an Pin 4
#define Echopin 6 // Der Echo-Anschluss des Ultraschallmoduls an Pin 5
#define Triggerpin 5 // Der Trigger-Anschluss des Ultraschallmoduls an Pin 6
#define Servopin 11 // Der Anschluss des Servomotors an Pin 11
// Definieren von Einstellungen zur Kalibrierung des Magnetometers
#define CALIBRATION_SAMPLES 3000 // Anzahl der Aufnahmen bei der Kalibrierung des Magnetometers
#define CRB_REG_M_1_3GAUSS 0x20 // Einstellung der Genauigkeit des Magnetometers auf +- 1.3 Gauß
#define CRA_REG_M_220HZ 0x1C // Einstellung der Aktualisierungsrate des Magnetometers auf 220 Hz
// Einbinden des Ultraschallsensors mit der NewPing-Bibliothek
NewPing sonar(Echopin, Triggerpin, 150);
// Einbinden des Motors mit der ZumoMotors-Bibliothek
ZumoMotors motors;
// Einbinden des bluetooth-Moduls mit der SoftwareSerial-Bibliothek
SoftwareSerial btSerial(Sendepin, Empfangspin);
// Einbinden des Buttons mit der internen Pushbutton-Bibliothek
Pushbutton button(ZUMO_BUTTON);
// Einbinden des Magnetometers mit der LSM303-Bibliothek
LSM303 compass;
// Definition von wichtigen Zahlenwerten:
// Variablen zum Rechnen
float schallgeschwindigkeit = 331.5 + 0.6 * 20; // Berechnung der Schallgeschwindigkeit v
// Konstanten für Fortbewegungen
const int MINIMALER_ABSTAND = 9; // Definiert den minimalen Abstand, ab dem der Roboter noch fahren soll (in der Einheit cm)
const int HOECHSTABSTAND = 18; // Definiert den maximalen Abstand zur linken Wand, ab dem er diese noch als Wand und nicht als Lücke wahrnehmen soll (in Einheit cm)
const float normalerweg = 5; // Definition des Weges, die der Roboter vowärts fahren soll, falls keine Unaufäliigkeiten entdeckt wurden (in Einheit cm)
const float Halbe_Roboterbreite = 5.5; // Definition der Sicherheitskonstanten, die der Roboter beim um die Ecke fahren noch vorwärts fahren soll (in Einheit cm)
const float Konstante = 2; // Konstante, die der Roboter an vielen Stellen als Pufferkonstante verwenden soll (in Einheit cm)
// Variablen für die Bewegungen
const int DEVIATION_THRESHOLD = 1; // Erlaubte Abweichung in Grad relativ zur bei der Drehung errechneten Zielrichtung
const int SPEED = 90; // Grundfaktor zur Einstellung der Drehgeschwindigkeit
const int TURN_BASE_SPEED = 75; // Zusätzlicher Summand zur Einstellung der Drehgeschwindigkeit
// Variablen für die Abstandsmessung
float min_gemessene_distanz; // zum Vergleich wird der Wert zunächst auf den Minimalabstand gesetzt
float min_gemessene_servoposition; // dieser Wert wird auf den minimal zu erreichenden Wert für den Servo gesetzt, da so später verglichen werden kann
// Variablen für Zeiten- Streckenaufnahme
float Motorgesamtzeit = 0; // Variable, die die Zeiten, die der Motor geradeausfährt, aufaddiert => Dadurch ist es möglich, die Strecke aufzunehmen (Zählt in ms)
float Motorgesamtstrecke = 0; // Variable, die die Stercken, die der Motor geradeausfährt, aufaddiert => Dadurch ist es möglich, die Strecke aufzunehmen
const long Intervall = 60000; // Definition der Konstanten, die mir das Intervall angibt, nachdem das Modul etwas senden soll, in diesem Fall einmal jede Sekunde
long previousMillis = 0; // Definition der Variablen für das Zählen der Zeit
int Start = 0; // Variable, der Roboter und die Zeitnahme erst starten, wenn das entsprechende Kommando gegeben wurde
unsigned long zeitbisstart = 0; // Variable zum Zurücksetzen und damit neuen Zählen der Millisekunden ab dem Startsignal
unsigned long zeitamende = 0; // Variable zum Speichern der Zeit, die bei der Betätigung des Notstopps vergangen ist
String btData; // Variable zum späteren Start der Zeitaufnahme und des Roboters
// Variablen für if-Abfragen
// int wand = 0; // Variable für das Finden einer Wand; bevor das Fahrzeug das erste mal auf eine Wand stößt ist dieser Wert false; ist einmal eine Wand gefunden, wird der Wert dauerhaft auf true gesetzt
int linke_Wand; // Initialisieren eines Zahlenwerts, anhand dessen später unterschieden werden kann, ob in dem Programmdurchlauf eine Wand links gefunden wurde oder nicht
int winkelzaehler = 0; // Definition der Variable, die nachzählt, um wie viel Grad sich der Roboter sich von der Ausgangsrichtung weggedreht hat
// Funktion zum Ansteuern der jeweiligen Servoposition
void softServoWrite(int angle, long servoDelay)
{
int pulsewidth = map(angle, 0, 180, 544, 2400); // Initialisierung und Berechnung der Pulsweite in Mikrosekunden abhängig von der einer Winkeleinstellung zwischen 0 und 180 Grad
do {
digitalWrite(Servopin, HIGH); // Unter Spannung setzen des Servos
delayMicroseconds(pulsewidth); // Je nach Winkel gewisse Pulsweite warten
digitalWrite(Servopin, LOW); // Spannungszufuhr beenden
delay(20); // Warten von 20 Mikrosekunden
servoDelay -= 20; // Abziehen eines Wertes von 20 von der eingestellten Dauer, die der Servo in die vorgesehene Position gebracht werden soll
}
while(servoDelay >=0); //Die do-Schleife wird so lange durchgeführt, bis die vorgesehene Einstellungsdauer überschritten ist
}
// so wie es hier steht, kann es vor den Setup geschrieben werden, will man die Funktion im loop anwenden, dann muss nur "abstandlinks(max_gemessene_distanz, max_gemessene_servoposition)" eingefügt werden; die &-Zeichen müssen vor den Variablen stehen, damit man nach Beendigung der Funktionen im loop mit den in der Funktion veränderten Werten weiterarbeiten kann, siehe auch http://forum.arduino.cc/index.php?topic=154412.0
void abstandlinks(int &linke_Wand)
{
float Werte_links[8] = {0,0,0,0,0,0,0,0}; // Initialisiere ein Array mit so vielen verschiedene verschiedenen Einträgen wie Positionen, die durchgegangen werden,; das Array, soll in der folgenden Schleife gefüllt werden
for(int j = 0; j<=7; j++)
{
Serial.println(Werte_links[j]);
}
// Überprüfe, ob sich links des Roboters eine Wand befindet: Messe dazu bis zu einem Winkel von 20° und schaue dir die Abstandswerte an (mache dies ruhig kleinschrittig)
for(int pos = 0; pos <= 70; pos += 10) // erste 60 Grad wegen (Mindestabstand * tan(70) = etwa Mindestbreite des Wegs (10° weniger überprüfen wegen Austtrahlwinkel des Ultraschalls) ==> es wird also überprüft, ob der Weg breit genug ist, um vernünftig hindurch zu fahren
{
softServoWrite(pos, 100);
delay(15); // Es soll 15 ms gewartet werden, damit er seine Position erreicht
// for-Schleife, um mehr als einen Messwert pro Position zu haben (in diesem Fall 5), der größte Abstand wird gespeichert
float dcm; // Initialisiert einen Wert vom Typ float, in dem später der gemittelte Abstand für die jeweilige Servoposition gespeichert wird
int anzahl_werte = 11; // Anzahl der aufgenommenen Werte pro Servoposition
float unsortierter_wert[anzahl_werte]; // Initialisiere ein Array mit 'Anzahl_Werte' verschiedenen Einträgen, die in der folgenden for-Schleife gefüllt werden
for(int durchlauf = 0; durchlauf < anzahl_werte; durchlauf ++) // Initialisierung der Laufvariablen; solange bis 'Anzahl_Werte' Durchläufe gemacht wurden (d. h. das Array gefüllt wurde); nach jedem Durchlauf wird die Laufvariable um 1 erhöht
{
// Aussenden und Verarbeiten der Daten aus dem Ultraschallsensor
float tUs = sonar.ping(); //tUs ist Signal aus der Rückantwort des Sensors in Mikrosekunden (Zeit zwischen Aussendung und Empfang des Signals); Die function gibt direkt einen Wert in cm aus
// Umrechnen der gemessenen Mikrosekunden in cm, ginge auch mit function aus der Standard-Library. Diese ist jedoch sehr ungenau, daher wird die NewPing-Library verwendet
float t = tUs / 1000.0 / 1000.0 ; // t ist Signal umgerechnet in Sekunden
float d = t / 2.0 * schallgeschwindigkeit; // Der Abstand d zum Objekt wird aus der Zeit und der Schallgeschwindigkeit berechnet
float dcm_durchlauf = d * 100; // Der Abstand wird von m in cm umgerechnet
// Speicherung des gemessenen Wertes auf eine Position des Arrays
if(dcm_durchlauf < 0.5) // Abfrage, ob der gemessene Wert zu klein ist (hier 0.5 genommen, da der Abstand aufgrund der Geometrie des Roboters nie erreicht werden kann, vorher fährt er gegen die Wand)
{
unsortierter_wert[durchlauf] = 100; // Ist der Wert zu klein, so wird er auf 100 korrigiert
}
else
{
unsortierter_wert[durchlauf]=dcm_durchlauf; // Ist der Wert nicht so klein, dann wird er wie oben an die jeweilige Position im Array gespeichert
}
delay(15); // Wartezeit, da der Sensor nicht schnell genug ist (durch Probieren: nicht weniger als 15 ms)
}
// Sortierung der Werte der Größe nach, beginnend mit dem kleinsten Wert; kopiert von: http://www.hackshed.co.uk/arduino-sorting-array-integers-with-a-bubble-sort-algorithm/
for(int i=0; i<(anzahl_werte-1); i++)
{
for(int o=0; o<(anzahl_werte-(i+1)); o++)
{
if(unsortierter_wert[o] > unsortierter_wert[o+1]) // Falls im unsortierten Array vorherige Wert größer ist als der nachfolgende, ...
{
float t = unsortierter_wert[o]; //..., dann setze den größeren Wert auf eine freie Variable,
unsortierter_wert[o] = unsortierter_wert[o+1]; //..., setze den kleineren Wert auf den jetzt freien Slot des größeren Werts
unsortierter_wert[o+1] = t; //..., setze den Eintrag der freien Variable auf den freien Slot des vorherigen kleineren Werts => jetzt haben die Werte die Plätze getauscht
// die for-Schleife mit Variabler o sorgt dafür, dass das Array einmal durchlaufen und durchgetauscht wird
// allerdings kann es sein, dass der hinterste Wert im unsortierten Array der höchste ist, daher muss man diese Prozedur 'Anzahl_Werte' oft machen => dafür sorgt die andere for-Schleife
}
}
}
// Zur Genauigkeit mittele über die mittleren fünf Werte, so werden gelegentliche und seltene Fehlmessungen in beide Richtungen herausgestrichen
dcm = (unsortierter_wert[(anzahl_werte-1)/2-2] + unsortierter_wert[(anzahl_werte-1)/2-1] + unsortierter_wert[(anzahl_werte-1)/2] + unsortierter_wert[(anzahl_werte-1)/2+1] + unsortierter_wert[(anzahl_werte-1)/2+2])/5;
Werte_links[pos/10]=dcm; //schreibe den gemessenen Wert in das vor der for-Schleife definierte Array
// Überprüfen der Abstandsergebnisse, d. h. Ausgabe im seriellen Monitor
Serial.print(pos); // Gebe Position aus
Serial.print("; "); // Sorge für Abstand zwischen den Werten
Serial.println(dcm, 2); // Gebe den Abstand in cm aus
}
for(int j = 0; j<=7; j++)
{
Serial.println(Werte_links[j]);
}
delay(100);
if((Werte_links[0] > HOECHSTABSTAND) && (Werte_links[1] > HOECHSTABSTAND) && (Werte_links[2] > HOECHSTABSTAND) && (Werte_links[3] > HOECHSTABSTAND) && (Werte_links[4] > HOECHSTABSTAND) && (Werte_links[5] > HOECHSTABSTAND) && (Werte_links[6] > HOECHSTABSTAND) && (Werte_links[7] > HOECHSTABSTAND) ) // Falls alle gemessenen Werte höher als der Höchstabstand sind, also keine Wand links zu finden ist, ...
{
linke_Wand = 1; // ,... dann setze den Wert, der anzeigen soll, ob links eine Wand gefunden wurde oder nicht, auf 1
}
}
// so wie es hier steht, kann es vor den Setup geschrieben werden, will man die Funktion im loop anwenden, dann muss nur "abstandgesamt(min_gemessene_distanz, min_gemessene_servoposition)" eingefügt werden; die &-Zeichen müssen vor den Variablen stehen, damit man nach Beendigung der Funktionen im loop mit den in der Funktion veränderten Werten weiterarbeiten kann, siehe auch http://forum.arduino.cc/index.php?topic=154412.0
void abstandgesamt(float &min_gemessene_distanz, float &min_gemessene_servoposition)
{
// Servo geht jetzt die Positionen von 0° bis 180° durch und überprüft auf den
for(int pos = 0; pos <= 180; pos += 10) // Es reicht aufgrund des Öffnungswinkels des Sensors aus, dass nicht jeder Werte gemessen werde muss => in diesem Fall nur jeder fünfte
{
softServoWrite(pos, 100);
delay(15); // Es soll 15 ms gewartet werden, damit er seine Position erreicht
// for-Schleife, um mehr als einen Messwert pro Position zu haben (in diesem Fall 11), der größte Abstand wird gespeichert
int durchlauf = 0; // Zurücksetzen des Zählers für die for-Schleife für jede Servoposition
float dcm = 0; // Setzt für jede Servoposition den höchsten Abstand auf 0, sodass dieser später mit den gemessenen Werten verglichen werden kann
int anzahl_werte = 11; // Anzahl der aufgenommenen Werte pro Servoposition
float unsortierter_wert[anzahl_werte]; // Initialisiere ein Array mit 'Anzahl_Werte' verschiedenen Einträgen, die in der folgenden for-Schleife gefüllt werden
for(durchlauf = 0; durchlauf < anzahl_werte; durchlauf ++)
{
// Aussenden und Verarbeiten der Daten aus dem Ultraschallsensor
float tUs = sonar.ping(); //tUs ist Signal aus der Rückantwort des Sensors in Mikrosekunden (Zeit zwischen Aussendung und Empfang des Signals);
// Umrechnen der gemessenen Mikrosekunden in cm, ginge auch mit function aus der library. Diese ist jedoch sehr ungenau
float t = tUs / 1000.0 / 1000.0 ; // t ist Signal umgerechnet in Sekunden
float d = t / 2.0 * schallgeschwindigkeit; // Der Abstand d zum Objekt wird aus der Zeit und der Schallgeschwindigkeit berechnet
float dcm_durchlauf = d * 100; // Der Abstand wird von m in cm umgerechnet
// Falls der Einfallswinkel des Ultraschalls zur Wand in einem ungünstigen Bereich liegt, kann es sein, dass der Sensor nichts misst und 0.00 als Abstand ausgibt.
// Diese Werte werden auf 100 gesetzt, da aufgrund der Ausmaße des Roboters ein Wert kleiner als 0,5 nicht möglich ist
// Da hier nur eine Abfrage nach unten geschieht, sind Werte, die zu groß sind, hier egal (man könnte also auch 1000 oder etwas anderes nehmen)
if(dcm_durchlauf < 0.5) // Abfrage, ob der gemessene Wert zu klein ist (hier 0.5 genommen, da der Abstand aufgrund der Geometrie des Roboters nie erreicht werden kann, vorher fährt er gegen die Wand)
{
unsortierter_wert[durchlauf] = 100; // Ist der Wert zu klein, so wird er auf 100 korrigiert
}
else
{
unsortierter_wert[durchlauf]=dcm_durchlauf; // Ist der Wert nicht zu klein, dann wird er wie oben an die jeweilige Position im Array gespeichert
}
delay(15); // Wartezeit, da der Sensor nicht schnell genug ist
}
// Sortierung der Werte, kopiert von: http://www.hackshed.co.uk/arduino-sorting-array-integers-with-a-bubble-sort-algorithm/
for(int i=0; i<(anzahl_werte-1); i++)
{
for(int o=0; o<(anzahl_werte-(i+1)); o++)
{
if(unsortierter_wert[o] > unsortierter_wert[o+1])
{
float t = unsortierter_wert[o];
unsortierter_wert[o] = unsortierter_wert[o+1];
unsortierter_wert[o+1] = t;
}
}
}
// Zur Genauigkeit mittele über die mittleren fünf Werte
dcm = (unsortierter_wert[(anzahl_werte-1)/2-2] + unsortierter_wert[(anzahl_werte-1)/2-1] + unsortierter_wert[(anzahl_werte-1)/2] + unsortierter_wert[(anzahl_werte-1)/2+1] + unsortierter_wert[(anzahl_werte-1)/2+2])/5;
// Abfrage, ob der gemessene Wert bei der jeweiligen Sensorposition über dem oben definierten Mindestabstand liegt
if (dcm >= MINIMALER_ABSTAND) // Falls der Abstand groß genug ist, ...
{
}
// Anzeige einer Warnmeldung, falls der Abstand zu gering ist und Speicherung der Servoposition, bei der der Abstand zuerst zu hoch war
else // Falls der Abstand zu klein ist, ...
{
// ... Speichern des Abstandswerts als neuen minimalen Abstand und der dazugehörigen Servoposition
// Hier Abfrage, ob der Wert der kleinste gemessene ist, oder ob es bei einer vorherigen Servoposition bereits einen tieferen gegeben hat
if(dcm < min_gemessene_distanz) // Falls der aktuelle Wert der bis dahin tiefste gemessene Wert ist, ...
{
// ... Speichern der gemessenen Distanz
min_gemessene_distanz = dcm;
// ... Speichern der aktuellen Servoposition
min_gemessene_servoposition = pos;
}
}
}
}
// so wie es hier steht, kann es vor den Setup geschrieben werden, will man die Funktion im loop anwenden, dann muss nur "vorwaertsfahren(strecke_vorwaerts)" eingefügt werden
void vorwaertsfahren(float strecke_vorwaerts)
{
float zeit_vorwaerts; // definiere die Variable, in der später die Zeit umgerechnet werden soll, die das Fahrzeug vorwärts fahren soll
zeit_vorwaerts = strecke_vorwaerts/10.6 * 1000; // Berechnung der Zeit, die vorwärts gefahren werden muss
motors.setSpeeds(106, 100); // In Bewegung setzen der beiden Motoren
delay(zeit_vorwaerts); // Zeit, die der Roboter vorwärts fahren soll (abhängig von der zu fahrenden Strecke), wird oben bereits umgerechnet
motors.setSpeeds(0, 0); // Stoppen der Motoren
Motorgesamtzeit = Motorgesamtzeit + zeit_vorwaerts; // Aufaddieren der Zeit, die sich der Roboter geradlinig bewegt
Motorgesamtstrecke = Motorgesamtstrecke + strecke_vorwaerts; // Aufaddieren der Strecke, die sich der Roboter geradlinig bewegt
Serial.print("Motorgesamtzeit: "); // Verdeutlichung der nachfolgend ausgegebenen Variablen
Serial.println(Motorgesamtzeit); // Ausgabe der Variablen in der gleichen Zeile mit anschließendem Zeilenumbruch
Serial.print("Motorgesamtstrecke: "); // Verdeutlichung der nachfolgend ausgegebenen Variablen
Serial.println(Motorgesamtstrecke); // Ausgabe der Variablen in der gleichen Zeile mit anschließendem Zeilenumbruch
}
void rueckwaertsfahren(float strecke_rueckwaerts)
{
float zeit_rueckwaerts; // definiere die Variable, in der später die Zeit umgerechnet werden soll, die das Fahrzeug vorwärts fahren soll
zeit_rueckwaerts = strecke_rueckwaerts/10.6 * 1000; // Berechnung der Zeit, die vorwärts gefahren werden muss
motors.setSpeeds(-106, -100); // In Bewegung setzen der beiden Motoren
delay(zeit_rueckwaerts); // Zeit, die der Roboter vorwärts fahren soll (abhängig von der zu fahrenden Strecke), wird oben bereits umgerechnet
motors.setSpeeds(0, 0); // Stoppen der Motoren
Motorgesamtzeit = Motorgesamtzeit + zeit_rueckwaerts; // Aufaddieren der Zeit, die sich der Roboter geradlinig bewegt
Motorgesamtstrecke = Motorgesamtstrecke + strecke_rueckwaerts; // Aufaddieren der Strecke, die sich der Roboter geradlinig bewegt
Serial.print("Motorgesamtzeit: "); // Verdeutlichung der nachfolgend ausgegebenen Variablen
Serial.println(Motorgesamtzeit); // Ausgabe der Variablen in der gleichen Zeile mit anschließendem Zeilenumbruch
Serial.print("Motorgesamtstrecke: "); // Verdeutlichung der nachfolgend ausgegebenen Variablen
Serial.println(Motorgesamtstrecke); // Ausgabe der Variablen in der gleichen Zeile mit anschließendem Zeilenumbruch
}
void drehung_rechts(float Drehwinkel)
{
// Initialisieren von Variablen zur Erfassung und Speicherung von Richtungen
float heading; // in dieser Variablen wird die aktuelle Richtung gespeichert
static float target_heading; // hier wird die Variable für die Zielrichtung definiertn und
float relative_heading; // In dieser Variablen wird die Differenz zwischen der aktuellen und der Zielrichtung gespeichert
int speed; // Initialisierung der Variablen für die Drehgeschwindigkeit beim Suchen der richtigen Richtung
// Füllen der oben initialisierten Variablen
heading = averageHeading(); // die aktuelle Richtung des Roboters wird aus dem Durchschnitt von 10 Messungen gemacht (siehe Funktion dazu) und hier gespeichert
target_heading = fmod(heading + Drehwinkel,360); // Die Zielrichtung wird aus der aktuellen Richtung addiert mit der Drehrichtung berechnet, da Rechtsdrehungen positiv hinzugezählt werden. Dabei sind nur Wert zwischen 0 und 360 erlaubt
relative_heading = relativeHeading(heading, target_heading); // Die Differenz von Zielrichtung und aktueller Richtung wird gemäß der Funktion berechnet und hier gespeichert
// Drehe dich nun in Richtung der Zielrichtung und vergleiche immer wieder die aktualisierte Richtung mit der Zielrichtung. Mache dies so lange, bis Richtung und Zielrichtung bis auf einen bestimmten vorher festgelegten Wert (hier: 1 Grad) übereinstimmen
do
{
// Um eine viel zu weite Drehung zu vermeiden, verlangsamt sich die Drehgeschwindigkeit, je näher der Roboter an der Zielrichtung ist. Dies wird wie folgt erreicht:
// Die obige Variable für die Drehgeschwindigkeit wird durch die zu Beginn festgelegte Grundgeschwindigkeit und der Differenz der Richtungen festgelegt. Außerdem wird durch den Faktor 180 geteilt. Ist die Differenz gering, so ist auch die Drehgeschwindigkeit gering.
speed = SPEED*relative_heading/180;
// Wenn die Geschwindigkeit kleiner als Null ist, d. h. die Differenz zwischen aktueller Richtung und Zielrichtung negativ ist, dann...
if (speed < 0)
speed -= TURN_BASE_SPEED; // ... ziehe vom alten Geschwindigkeitswert den eingestellten zusätzlichen Drehgeschwindigkeitswert ab und speichere ihn neu => Ist die Differenz groß, so ist auch die Drehgeschwindigkeit groß; für kleine Differenzen ist sie klein
else
speed += TURN_BASE_SPEED; // Im anderen Fall ist die Geschwindigkeit größer als Null => Rechne hier die Geschwindigkeit dazu
motors.setSpeeds(1.06*speed, -speed); // Lasse die Motoren mit der berechneten Geschwindigkeit entgegengesetzt drehen. Ist die Geschwindigkeit kleiner als Null, so erfolgt eine Rechtsdrehung, sonst eine Linksdrehung
// Aktualisiere die Werte für die jetzige Richtung und die Differenz zwischen aktueller Richtung und Zielrichtung
heading = averageHeading(); // Aktualisiere den Wert der aktuellen Richtung
relative_heading = relativeHeading(heading, target_heading); // Aktualisiere die Differenz zur Zielrichtung
}
while(abs(relative_heading) >= DEVIATION_THRESHOLD); // Führe die do-Schleife, also erst Drehung und anschließend Überprüfung der der Werte, so lange durch, bis der Betrag der Differenz kleiner als ein vorher festgelegter Wert ist (hier: 1 Grad)
// Ist die Schleife abgebrochen, so schaut der Roboter im Rahmen einer gewissen Genauigkeit in Zielrichtung.
motors.setSpeeds(0,0); // Beende dann die Drehung, indem die Geschwindigkeit der beiden Motoren auf Null gesetzt wird
}
void drehung_links(float Drehwinkel)
{
// Initialisieren von Variablen zur Erfassung und Speicherung von Richtungen
float heading; // in dieser Variablen wird die aktuelle Richtung gespeichert
static float target_heading; // hier wird die Variable für die Zielrichtung definiertn und
float relative_heading; // In dieser Variablen wird die Differenz zwischen der aktuellen und der Zielrichtung gespeichert
int speed; // Initialisierung der Variablen für die Drehgeschwindigkeit beim Suchen der richtigen Richtung
// Füllen der oben initialisierten Variablen
heading = averageHeading(); // die aktuelle Richtung des Roboters wird aus dem Durchschnitt von 10 Messungen gemacht (siehe Funktion dazu) und hier gespeichert
target_heading = fmod(heading - Drehwinkel,360); // Die Zielrichtung wird durch die Differenz von aktueller Richtung und Drehwinkel berechnet, da Linksdrehungen negativ hinzugezählt werden. Dabei sind nur Wert zwischen 0 und 360 erlaubt
relative_heading = relativeHeading(heading, target_heading); // Die Differenz von Zielrichtung und aktueller Richtung wird gemäß der Funktion berechnet und hier gespeichert
// Drehe dich nun in Richtung der Zielrichtung und vergleiche immer wieder die aktualisierte Richtung mit der Zielrichtung. Mache dies so lange, bis Richtung und Zielrichtung bis auf einen bestimmten vorher festgelegten Wert (hier: 1 Grad) übereinstimmen
do
{
// Um eine viel zu weite Drehung zu vermeiden, verlangsamt sich die Drehgeschwindigkeit, je näher der Roboter an der Zielrichtung ist. Dies wird wie folgt erreicht:
// Die obige Variable für die Drehgeschwindigkeit wird durch die zu Beginn festgelegte Grundgeschwindigkeit und der Differenz der Richtungen festgelegt. Außerdem wird durch den Faktor 180 geteilt. Ist die Differenz gering, so ist auch die Drehgeschwindigkeit gering.
speed = SPEED*relative_heading/180;
// Wenn die Geschwindigkeit kleiner als Null ist, d. h. die Differenz zwischen aktueller Richtung und Zielrichtung negativ ist, dann...
if (speed < 0)
speed -= TURN_BASE_SPEED; // ... ziehe vom alten Geschwindigkeitswert den eingestellten zusätzlichen Drehgeschwindigkeitswert ab und speichere ihn neu => Ist die Differenz groß, so ist auch die Drehgeschwindigkeit groß; für kleine Differenzen ist sie klein
else
speed += TURN_BASE_SPEED; // Im anderen Fall ist die Geschwindigkeit größer als Null => Rechne hier die Geschwindigkeit dazu
motors.setSpeeds(1.06*speed, -speed); // Lasse die Motoren mit der berechneten Geschwindigkeit entgegengesetzt drehen. Ist die Geschwindigkeit kleiner als Null, so erfolgt eine Rechtsdrehung, sonst eine Linksdrehung
// Aktualisiere die Werte für die jetzige Richtung und die Differenz zwischen aktueller Richtung und Zielrichtung
heading = averageHeading(); // Aktualisiere den Wert der aktuellen Richtung
relative_heading = relativeHeading(heading, target_heading); // Aktualisiere die Differenz zur Zielrichtung
}
while(abs(relative_heading) >= DEVIATION_THRESHOLD); // Führe die do-Schleife, also erst Drehung und anschließend Überprüfung der der Werte, so lange durch, bis der Betrag der Differenz kleiner als ein vorher festgelegter Wert ist (hier: 1 Grad)
// Ist die Schleife abgebrochen, so schaut der Roboter im Rahmen einer gewissen Genauigkeit in Zielrichtung.
motors.setSpeeds(0,0); // Beende dann die Drehung, indem die Geschwindigkeit der beiden Motoren auf Null gesetzt wird
}
// Diese function wird ausgeführt, sobald einer der Taster gedrückt wird, also der Roboter unerwarteterweise gegen ein Hindernis gefahren ist
void Notstop()
{
motors.setSpeeds(0,0); // Schalte die Motoren auf Null, sodass sich der Roboter nicht mehr bewegt
zeitamende = millis(); // Speichere die Anzahl der Millisekunden, die zum Zeitpunkt der Betätigung des Notstopps vergangen sind
long durchzaehlen = 0; // Definiere eine sehr große Variable, die später in einer while Schleife hochgezählt wird
long zuruecksetzen = 10000000; // Definiere eine sehr große Variable, die später von der obigen abgezogen wird, sodass das Ausgeben über das bluetooth-Modul ausgelöst wird
while(durchzaehlen < 20000000) // Solange die Variable durchzaehlen kleiner als 2 Milliarden ist, ...
{
long ausloesen = durchzaehlen - zuruecksetzen; // ... definiere eine Variable, die später eine if-Abfarage zur Ausgabe von Werten auslöst
if(ausloesen >= 0){ // Wenn so weit gezählt wurde, dass ausloesen null ergibt (also bis 1 Milliarde), dann...
btSerial.print("Endzeit in Sekunden: "); // ... gebe über Bluetooth die Endzeit in Sekunden aus
btSerial.println((zeitamende-zeitbisstart)/1000); // ... gebe über Bluetooth in der gleichen Zeile die Endzeit in Sekunden aus
btSerial.print("Am Ende zurückgelegte Strecke in cm: "); // ... gebe über Bluetooth die zurückgelegte Strecke in Zentimetern aus
btSerial.println(Motorgesamtstrecke); // ... gebe über Bluetooth in der gleichen Zeile die zurückgelegte Strecke in Zentimetern aus
btSerial.print("Endstand des Winkelzählers: "); // ... gebe gib über Bluetooth die zurückgelegte Strecke in Zentimetern aus
btSerial.println(winkelzaehler); // ... gebe über Bluetooth in der gleichen Zeile die zurückgelegte Strecke in Zentimetern aus
durchzaehlen = 0; // ... dann setze außerdem die Variable "durchzaehlen" auf null zurück, sodass die while-Schleife und das hochzählen von neuem beginnt => es entsteht eine Endlosschleife
}
durchzaehlen++; // ... erhöhe die Variable zum Durchzählen am Schluss immer um 1
}
}
// Mithilfe dieser Befehlskette wird aus den ermittelten Daten für die x- und y-Achse des Magnetometers eine Richtung berechnet. Sie ist aus dem Beispielsketch zur Verwendung des Magnetometers entnommen.
// Dabei wird explizit darauf hingewiesen, dass die z-Achse aufgrund von äußeren Einflüssen nicht zur Berechnung hinzugefügt wird. Es wird davon ausgegangen, dass der Roboter eben steht.
template <typename T> float heading(LSM303::vector<T> v)
{
float x_scaled = 2.0*(float)(v.x - compass.m_min.x) / (compass.m_max.x - compass.m_min.x) - 1.0; // Skalierung der x-Komponente; v.x ist der aktuelle vom Kompass ermittelte Wert der x-Komponente; die anderen Variablen folgen aus der Kalibrierung
float y_scaled = 2.0*(float)(v.y - compass.m_min.y) / (compass.m_max.y - compass.m_min.y) - 1.0; // Skalierung der y-Komponente; v.y ist der aktuelle vom Kompass ermittelte Wert der y-Komponente; die anderen Variablen folgen aus der Kalibrierung
float angle = atan2(y_scaled, x_scaled)*180 / M_PI; // Zusammenfügung und Berechnung der Richtung aus dem Arcustangens der beiden skalierten Komponenten und einem Quotienten zur Umrechnung von Radiant in Grad
if (angle < 0) // Es sollen nur Werte zwischen 0 und 360 ausgegeben werden. Ist die berechnete Richtung also kleiner, dann...
angle += 360; // ... rechne den Wert 360 hinzu
return angle; // Gebe die Richtung aus
}
// Mithilfe dieser Funktion wird die aktuelle Position des Roboters bestimmt. Dazu wird der Kompass zehn mal ausgelesen und dann gemittelt. Die Funktion ist dem Beispielsketch entnommen
float averageHeading()
{
LSM303::vector<int32_t> avg = {0, 0, 0}; // Initialisiere einen Vektor für die Richtung, der zunächst an allen Stellen null ist; hier wird int32_t verwendet, da insgesamt höhere Werte angenommen werden können und hier 32 Bits statt 16 Bits gefüllt werden können
for(int i = 0; i < 10; i ++) // Führe diese Schleife zehn mal aus
{
compass.read(); // Lese die Daten des Kompass aus
avg.x += compass.m.x; // Füge die Daten der x-Komponente einer der x-Komponente des zu Beginn der Funktion erstellten Vektors zu
avg.y += compass.m.y; // Füge die Daten der y-Komponente einer der y-Komponente des zu Beginn der Funktion erstellten Vektors zu
}
avg.x /= 10.0; // Mittele die hochgezählte x-Komponente für einen durchschnittlichen Wert
avg.y /= 10.0; // Mittele die hochgezählte y-Komponente für einen durchschnittlichen Wert
return heading(avg); // Gebe die resultierende mittlere Richtung aus, die mithilfe der hier ermittelten Werte und der Template-Funktion berechnet werden
}
// Funktion zur Berechnung der Differenz zwischen der aktuellen und der Zielrichtung. sie ist aus dem Beispielsketch zur Verwendung des Magtnetometers entnommen
float relativeHeading(float heading_from, float heading_to) // an die Funktion werden zwei Parameter übergeben: der erste stellt die aktuelle Richtung dar, der zweite die Zielrichtung
{
float relative_heading = heading_to - heading_from; // Initialisiere eine Variable vom Typ float und befülle sie direkt mit der Differenz aus Zielrichtung und aktueller Richtung
// Sorge dafür, dass die Differenz immer in einem Bereich zwischen -180 und 180 liegt
if (relative_heading > 180) // Wenn die Differenz einen Wert größer als 180 besitzt, dann...
relative_heading -= 360; // ... ziehe 360 davon ab
if (relative_heading < -180) // analoges Vorgehen für einen Wert kleiner als -180
relative_heading += 360;
return relative_heading; // Gebe die erstellte float-Variable aus
}
void setup() {
// Initialisieren des seriellen Schnittstelle des Bluetooth-Moduls zur Kontrolle von Messwerten
btSerial.begin(9600); // Einbinden des Bluetoothsignals und Einstellung der baud-Rate (analog zur normalen seriellen Datenübertragung)
btSerial.println("bluetooth erreichbar"); // Schreiben einer Zeile mit anschließendem Umbruch (analog zur normalen seriellen Datenübertragung)
pinMode(Servopin, OUTPUT); // Setze den Servopin als Output
// Initialisierungen für den Notstop
pinMode(TasterPin, INPUT); // Setzt den TasterPin als Input
digitalWrite(TasterPin, HIGH); // Aktiviert den Pull-Up-Widerstand am TasterPin
attachInterrupt(0, Notstop, FALLING); // Initialisierung des Interrupts
LSM303::vector<int16_t> running_min = {32767, 32767, 32767}, running_max = {-32767, -32767, -32767}; // Einstellungen für den Vektor, über den später die Richtungen ermittelt werden
Wire.begin(); // Initialisiere die Wire-Biblithek zur Kommunikation mit dem I2C-Bus, über den das Magnetometer angeschlossen ist
compass.init(); // Initialisiere den Kompass
compass.enableDefault(); // Aktiviere den Kompass
compass.writeReg(LSM303::CRB_REG_M, CRB_REG_M_1_3GAUSS); // Stelle den Kompass so ein, dass er eine Sinsitivität von +/- 1.3 Gauss besitzt
compass.writeReg(LSM303::CRA_REG_M, CRA_REG_M_220HZ); // Stelle den Kompass so ein, dass er eine Update-Rate von 220 Hz besitzt
button.waitForButton(); // Warte bis der Button gedrückt wird, bis der Programmablauf fortgesetzt wird
btSerial.println("starting calibration"); // Gebe via Bluetooth aus, dass die Kalibrierung gestartet wird
// Um das Magnetometer zu kalibrieren, dreht sich der Roboter und sucht dabei in alle Richtungen das Maximal- und Minimalwerte der einzelnen Komponenten. Damit werden Offsets korrigiert
motors.setSpeeds(1.06*100, -100); // Starte die Motoren, so dass sich der Roboter dreht
for(int index = 0; index < CALIBRATION_SAMPLES; index ++) // Führe diese Schleife so oft durch, wie zu Beginn des Programms eingestellt (hier: 3000 mal)
{
compass.read(); // Lies den Kompass aus und speichere es in compass.m
running_min.x = min(running_min.x, compass.m.x); // Falls die aktuelle x-Komponente kleiner ist als die bisherige Kleinste, so überschreibe Letztere
running_min.y = min(running_min.y, compass.m.y); // Falls die aktuelle y-Komponente kleiner ist als die bisherige Kleinste, so überschreibe Letztere
running_max.x = max(running_max.x, compass.m.x); // Falls die aktuelle x-Komponente größer ist als die bisherige Größte, so überschreibe Letztere
running_max.y = max(running_max.y, compass.m.y); // Falls die aktuelle y-Komponente größer ist als die bisherige Größte, so überschreibe Letztere
btSerial.println(index); // Gebe die Nummer des aktuellen Durchlaufs via Bluetooth aus
delay(50); // Warte 50 Millisekunden zwischen zwei Kompassmessungen
}
motors.setSpeeds(0,0); // Stoppe nach Beendigung der Kalibrierung die Motoren
btSerial.print("max.x "); // Verdeutlichung der nachfolgend ausgegebenen Variablen
btSerial.println(running_max.x); // Ausgabe der Variablen in der gleichen Zeile mit anschließendem Zeilenumbruch
btSerial.print("max.y "); // Verdeutlichung der nachfolgend ausgegebenen Variablen
btSerial.println(running_max.y); // Ausgabe der Variablen in der gleichen Zeile mit anschließendem Zeilenumbruch
btSerial.print("min.x "); // Verdeutlichung der nachfolgend ausgegebenen Variablen
btSerial.println(running_min.x); // Ausgabe der Variablen in der gleichen Zeile mit anschließendem Zeilenumbruch
btSerial.print("min.y "); // Verdeutlichung der nachfolgend ausgegebenen Variablen
btSerial.println(running_min.y); // Ausgabe der Variablen in der gleichen Zeile mit anschließendem Zeilenumbruch
compass.m_max.x = running_max.x; // Speichere die größte Komponente in x-Richtung in der zugehörigen Komponente für die Offsetberechnungen des Kompass
compass.m_max.y = running_max.y; // Speichere die größte Komponente in y-Richtung in der zugehörigen Komponente für die Offsetberechnungen des Kompass
compass.m_min.x = running_min.x; // Speichere die kleinste Komponente in x-Richtung in der zugehörigen Komponente für die Offsetberechnungen des Kompass
compass.m_min.y = running_min.y; // Speichere die kleinste Komponente in y-Richtung in der zugehörigen Komponente für die Offsetberechnungen des Kompass
delay(1000); // Warte 1 Sekunde
}
void loop() {
btData = btSerial.readString(); // auslesen des an das bluetooth-Modul geschickten Strings
if(btData == "Start"){ // Wenn die Zeichenfolge 'Start' gesendet wurde, dann...
Start = 1; // ... setze die Variable für den Start des Roboters auf 1, damit dieser losfährt
zeitbisstart = millis(); // ... speichere die bis hier vergangene Zeit seit dem Hochfahren des Arduinos in einer Variablen
}
if(Start == 1){
long currentMillis = millis() - zeitbisstart; // Variable für die seit dem Start des Arduinos vergangene Zeit (zählt die Zeit ab dem Startsignal)
// if-Abfrage zum ausgeben der vergangenen Zeit und des zurückgelegten Weges nach jeder Minute
if((currentMillis - previousMillis) >= Intervall){ // Wenn die Intervallzeit überschritten wurde,...
previousMillis = currentMillis; // ... dann setze zuerst die Variable auf den aktuellen Wert, damit erneut die Intervallzeit abgewartet wird
btSerial.print("Vergangene Zeit in min: "); // ... dann gib über Bluetooth die vergangene Zeit in Minuten aus
btSerial.println(currentMillis/60000); // ... dann gib über Bluetooth die vergangene Zeit in Minuten aus
btSerial.print("Zurückgelegte Strecke in cm: "); // ... dann gib über Bluetooth die zurückgelegte Strecke in Zentimetern aus
btSerial.println(Motorgesamtstrecke); // ... dann gib über Bluetooth die zurückgelegte Strecke in Zentimetern aus
}
// Gebe zur Kontrolle des Wertes zu Anfang eines Loop-Durchlaufs einmal den aktuellen Wert des Winkelzählers aus
btSerial.print("Winkelzähler: "); // Verdeutlichung der nachfolgend ausgegebenen Variablen
btSerial.println(winkelzaehler); // Ausgabe der Variablen in der gleichen Zeile mit anschließendem Zeilenumbruch
// Initialisierung der minimalen gemessenen Distanz und der minimalen gemesseenen Servoposition, ebenso für die maximalen Werte
min_gemessene_distanz = MINIMALER_ABSTAND; // zum Vergleich wird der Wert zunächst auf den Minimalabstand gesetzt
min_gemessene_servoposition = 0; // dieser Wert wird auf den minimal zu erreichenden Wert für den Servo gesetzt, da so später verglichen werden kann
linke_Wand = 0; // Setze den Wert zur Überprüfung, ob in dem Durchlauf links eine Wand gefunden wurde, zunächst zurück
softServoWrite(0, 1000); // Setze den Servo vor Beginn auf Nullposition
// Anfang: Fahrt unabhängig von Wand
if(winkelzaehler == 0)
{
// Anfang: Überprüfung auf Mindestabstand und Folgen
abstandgesamt(min_gemessene_distanz, min_gemessene_servoposition); // Überprüfe den Abstand in alle Richtungen auf Wände
delay(10); // Warte 10 Millisekunden
// Überprüfe, ob in diesem Scan-Durchlauf der Abstand irgendwann zu gering war
if((min_gemessene_servoposition >= 1) && (min_gemessene_servoposition <= 180)) // Falls der Abstand zu gering war, dann liegt, dann gilt: 0 <= 'min_gemessene_Servoposition' <= 180. Ist das der Fall, dann...
{
winkelzaehler = winkelzaehler + min_gemessene_servoposition; // Aktualisiere den Winkelzähler
// Durch die folgenden drei if-Abfragen wird realisiert, dass der Roboter nach der Drehung gerade auf die Wand schaut
if(min_gemessene_servoposition < 90) // Wenn die Servoposition zum kleinsten Abstand kleiner als 90 ist, d. h. die Wand links vom Roboter liegt, dann...
{
drehung_links(90-min_gemessene_servoposition); // ... drehe dich um 90 minus die Servoposition nach links
}
if(min_gemessene_servoposition > 90) // Wenn die Servoposition zum kleinsten Abstand kleiner als 90 ist, d. h. die Wand rechts vom Roboter liegt, dann...
{
drehung_rechts(min_gemessene_servoposition - 90); // ... drehe dich um die Servoposition -90 nach rechts
}
if(min_gemessene_servoposition = 90) // Wenn die Servoposition zum kleinsten Abstand gleich 90 ist, d. h. die Wand gerade vor dem Roboter liegt, dann mache nichts
{
}
delay(10); // Warte 10 Millisekunden
rueckwaertsfahren(MINIMALER_ABSTAND - min_gemessene_distanz + Konstante); // Fahre rückwärts, um dich von der Wand zu Entfernen; die beiden ersten Komponenten sorgen dafür, dass der Mindestabstand hergestellt ist, die Konstante dafür, dass bis dahin ein kleiner Puffer vorhanden ist
delay(10); // Warte 10 Millisekunden
drehung_rechts(90); // Drehe dich nach rechts, sodass du parallel zur jetzt links von dir liegenden Wand stehst
delay(10); // Warte 10 Millisekunden
}
else // Falls der Abstand in diesem Durchlauf kein mal zu gering war, ...
{
vorwaertsfahren(normalerweg); // ... fahre den vorher eingestellen Standardweg nach vorne
delay(10); // Warte 10 Millisekunden
}
// Ende: Überprüfung auf Mindestabstand und Folgen
softServoWrite(0, 1000); // Setze den Servo vor dem nächsten Durchlauf wieder auf die Nullposition
// Zurücksetzen der Vergleichswerte
min_gemessene_distanz = MINIMALER_ABSTAND;
min_gemessene_servoposition = 0;
}
// Ende: Fahrt unabhängig von Wand
// Anfang: Fahrt abhängig von Wand
if(winkelzaehler > 0) // Sobald der Winkelzähler nicht mehr 0 anzeigt, und damit an einer Wand entlangfährt, ...
{
// Anfang: Überprüfen auf Höchstabstand und Folgen
abstandlinks(linke_Wand); // Überprüfe zunächst den Abstand nach links, ob er zu hoch ist
delay(10); // Warte 10 Millisekunden
if(linke_Wand == 1) // Wenn auf der linken Seite keine Wand gefunden wurde, ...
{
vorwaertsfahren(2 * Halbe_Roboterbreite + Konstante); // ... dann fahre zunächst ein Stück vorwärts. Die Strecke setzt sich aus der Roboterlänge und der Pufferkonstanten zusammen
delay(10); // Warte 10 Millisekunden
// Hier muss nun überprüft werden, ob der Roboter sich wieder von der Wand lösen soll oder nicht, das heißt, ob er während der Drehung in den Gang irgendwann wieder in die Ausgangsrichtung schaut
if(winkelzaehler - 90 <= 0) // Falls der Winkelzähler in diesem Durchlauf kleiner oder gleich 0 wird, das heißt falls du zwischendurch in die Ausgangsrichtung schauen könntest ...
{
drehung_links(winkelzaehler); // ..., drehe dich nur um den Wert des Winkelzählers, damit du wieder in Anfangsrichtung schaust
delay(10); // Warte 10 Millisekunden
winkelzaehler = 0; // ..., dann setze den Winkelzähler wieder auf 0 zurück
vorwaertsfahren(normalerweg); // ..., fahre den normalen Weg vorwärts, da du in Ausgangsrichtung schaust und damit ab dem nächsten Durchlauf unabhängig von der Wand fährst
delay(10); // Warte 10 Millisekunden
}
else // Wenn du während der Drehung nicht in Ausgangsrichtung schaust, dann...
{
drehung_links(90); // ... drehe dich um 90 Grad nach links, sodass du durch vorwärtsfahren entweder in die Einbuchtung hinein fährst oder den Abstand zur Wand wieder verkürzt
delay(10); // Warte 10 Millisekunden
winkelzaehler = winkelzaehler - 90; // aktualisiere die den Winkelzähler, er ist dann weiterhin positiv
vorwaertsfahren(15); // ... fahre um einen gewissen Wert nach vorne, damit du im Gang, in den du einbiegen wolltest, stehst oder den Abstand zur Wand wieder verkürzt
delay(10); // Warte 10 Millisekunden
}
// Ende: Überprüfen auf Höchstabstand und Folgen
softServoWrite(0, 1000); // Setze vor dem nächsten Servordurchlauf den Servo wieder auf die Nullposition
// Überprüfe direkt noch einmal, ob der Abstand in irgendeine Richtung zu gering ist, damit der Roboter nicht nach links überprüft, wegen der Reflexion der Abstand zu hoch ist und er dann beim vorwärtsfahren gegen eine Wand fährt (siehe Video vom 27.10.)
// Dementsprechend überprüfe hier nur auf den Mindestabstand => ist dieser in eine Richtung zu gering, dann soll er sich wie sonst auch drehen, etc.; ist er nicht zu gering, dann soll der Roboter sich gar nicht bewegen, sodass direkt wieder auf eine Wand nach links in überprüft werden kann
abstandgesamt(min_gemessene_distanz, min_gemessene_servoposition); // Überprüfe den Abstand in alle Richtungen auf Wände
delay(10); // Warte 10 Millisekunden
// Überprüfe, ob in diesem Scan-Durchlauf der Abstand irgendwann zu gering war
if((min_gemessene_servoposition >= 1) && (min_gemessene_servoposition <= 180)) // Falls der Abstand zu gering war, dann liegt, dann gilt: 0 <= 'min_gemessene_Servoposition' <= 180. Ist das der Fall, dann...
{
winkelzaehler = winkelzaehler + min_gemessene_servoposition; // Aktualisiere den Winkelzähler
// Durch die folgenden drei if-Abfragen wird realisiert, dass der Roboter nach der Drehung gerade auf die Wand schaut
if(min_gemessene_servoposition < 90) // Wenn die Servoposition zum kleinsten Abstand kleiner als 90 ist, d. h. die Wand links vom Roboter liegt, dann...
{
drehung_links(90-min_gemessene_servoposition); // ... drehe dich um 90 minus die Servoposition nach links
}
if(min_gemessene_servoposition > 90) // Wenn die Servoposition zum kleinsten Abstand kleiner als 90 ist, d. h. die Wand rechts vom Roboter liegt, dann...
{
drehung_rechts(min_gemessene_servoposition - 90); // ... drehe dich um die Servoposition -90 nach rechts
}
if(min_gemessene_servoposition = 90) // Wenn die Servoposition zum kleinsten Abstand gleich 90 ist, d. h. die Wand gerade vor dem Roboter liegt, dann mache nichts
{
}
delay(10); // Warte 10 Millisekunden
rueckwaertsfahren(MINIMALER_ABSTAND - min_gemessene_distanz + Konstante); // Fahre rückwärts, um dich von der Wand zu Entfernen; die beiden ersten Komponenten sorgen dafür, dass der Mindestabstand hergestellt ist, die Konstante dafür, dass bis dahin ein kleiner Puffer vorhanden ist
delay(10); // Warte 10 Millisekunden
drehung_rechts(90); // Drehe dich nach rechts, sodass du parallel zur jetzt links von dir liegenden Wand stehst
delay(10); // Warte 10 Millisekunden
}
else // Falls der Abstand in diesem Durchlauf kein mal zu gering war, mache nichts
{
}
}
// Ende: Überprüfen auf Höchstabstand und Folgen
softServoWrite(0, 1000); // Setze vor dem nächsten Servordurchlauf den Servo wieder auf die Nullposition
if(linke_Wand == 0) // Wenn erkannt wurde, dass die Wand auf der linken Seite noch nahe genug am Roboter liegt, dann
{
// Anfang: Überprüfung auf Mindestabstand und Folgen
abstandgesamt(min_gemessene_distanz, min_gemessene_servoposition); // ... überprüfe den Abstand in alle Richtungen auf Wände
delay(10); // Warte 10 Millisekunden
// Überprüfe, ob in diesem Scan-Durchlauf der Abstand irgendwann zu gering war
if((min_gemessene_servoposition >= 1) && (min_gemessene_servoposition <= 180)) // Falls der Abstand zu gering war, dann liegt, dann gilt: 0 <= 'min_gemessene_Servoposition' <= 180. Ist das der Fall, dann...
{
winkelzaehler = winkelzaehler + min_gemessene_servoposition; // Aktualisiere den Winkelzähler
// Durch die folgenden drei if-Abfragen wird realisiert, dass der Roboter nach der Drehung gerade auf die Wand schaut
if(min_gemessene_servoposition < 90) // Wenn die Servoposition zum kleinsten Abstand kleiner als 90 ist, d. h. die Wand links vom Roboter liegt, dann...
{
drehung_links(90-min_gemessene_servoposition); // ... drehe dich um 90 minus die Servoposition nach links
}
if(min_gemessene_servoposition > 90) // Wenn die Servoposition zum kleinsten Abstand kleiner als 90 ist, d. h. die Wand rechts vom Roboter liegt, dann...
{
drehung_rechts(min_gemessene_servoposition - 90); // ... drehe dich um die Servoposition -90 nach rechts
}
if(min_gemessene_servoposition = 90) // Wenn die Servoposition zum kleinsten Abstand gleich 90 ist, d. h. die Wand gerade vor dem Roboter liegt, dann mache nichts
{
}
delay(10); // Warte 10 Millisekunden
rueckwaertsfahren(MINIMALER_ABSTAND - min_gemessene_distanz + Konstante); // Fahre rückwärts, um dich von der Wand zu Entfernen; die beiden ersten Komponenten sorgen dafür, dass der Mindestabstand hergestellt ist, die Konstante dafür, dass bis dahin ein kleiner Puffer vorhanden ist
delay(10); // Warte 10 Millisekunden
drehung_rechts(90); // Drehe dich nach rechts, sodass du parallel zur jetzt links von dir liegenden Wand stehst
delay(10); // Warte 10 Millisekunden
}
else // Falls der Abstand in diesem Durchlauf kein mal zu gering war, ...
{
vorwaertsfahren(normalerweg);// ..., dann fahre, da ja keine Wand vorliegt, eine vorher definierte Weglänge vorwärts und starte von Neuem
delay(10); // Warte 10 Millisekunden
}
// Ende: Überprüfung auf Mindestabstand und Folgen
}
}
// Ende: Fahrt abhängig von Wand
}
}
