129 lines
4.3 KiB
C++
129 lines
4.3 KiB
C++
#include "CodeRacer.h"
|
|
|
|
#include <ESP32Servo.h>
|
|
#include "esp32-hal-ledc.h"
|
|
|
|
//----- Werte für den Ultraschallsensor -----
|
|
#define US_STOP_ABSTAND_CM 20 // Wenn der gemessene Abstand kleiner ist, hält der CodeRacer an
|
|
#define US_MIN_ABSTAND_LI_RE 8 // Wenn der Unterschied zwischen linkem und und rechtem Abstand kleiner ist, dann drehe in dieselbe Richtugn wie vorher weiter
|
|
#define MAX_ANZAHL_DREHUNGEN 10 // Wenn der Coderacer sich schon so oft gedreht hat ohne eine Stelle zu finden, wo es Platz gibt - fahren wir mal ein Stück rückwärts ...
|
|
|
|
//----- Variablen, die wir brauchen um uns Werte zu merken ----
|
|
long abstand_vorn_cm, abstand_links_cm, abstand_rechts_cm;
|
|
enum drehrichtung {links=0, rechts};
|
|
drehrichtung drehung = links;
|
|
unsigned int anzahl_drehung = 0;
|
|
CodeRacer coderacer;
|
|
|
|
//---- Hier startet der Code zum Einstellen aller wichtigen Dinge. Setup() wird einmal ausgeführt. ----
|
|
void setup() {
|
|
// Monitor
|
|
Serial.begin(115200); // Serial Monitor aktivieren. Mit dem Monitor kann man sich Werte und Meldungen anzeigen lassen.
|
|
|
|
// CodeRacer initialisieren
|
|
coderacer.begin();
|
|
|
|
coderacer.servo_links();
|
|
delay(10);
|
|
coderacer.servo_rechts();
|
|
delay(10);
|
|
coderacer.servo_mitte();
|
|
|
|
anzahl_drehung = 0;
|
|
drehung = links;
|
|
|
|
}
|
|
|
|
//---- Hier startet unsere endlose Schleife - die immer wieder von vorn angefangen wird, wenn wir am Ende angekommen sind. Da ist unser "Fahr"Code drin, der den CodeRacer steuert
|
|
void loop() {
|
|
|
|
|
|
// Abstand messen -> nach vorn ... um zu sehen, ob was passiert messen wir immer ... auch wenn der Racer nicht fahren soll
|
|
abstand_vorn_cm = coderacer.abstand_messen();
|
|
|
|
// Abfragen ob der Racer fahren soll oder nicht ...
|
|
if(true == coderacer.start_stop()){
|
|
|
|
// Abstandssensor schon verstellen ... dann hat er das bis zur nächsten Messung auch geschafft
|
|
coderacer.servo_schwenk();
|
|
|
|
// Ist die Bahn frei?
|
|
if(abstand_vorn_cm < US_STOP_ABSTAND_CM){
|
|
// Nein! Der Abstand nach vorn ist kleiner als erlaubt!
|
|
// Racer anhalten
|
|
coderacer.anhalten();
|
|
// Nach links schauen!
|
|
coderacer.servo_links();
|
|
// Abstand messen und merken.
|
|
abstand_links_cm = coderacer.abstand_messen();
|
|
// Nach rechts schauen!
|
|
coderacer.servo_rechts();
|
|
// Abstand messen und merken.
|
|
abstand_rechts_cm = coderacer.abstand_messen();
|
|
|
|
|
|
// Ist der Abstand links oder rechts groß genug genug?
|
|
// Wenn nicht - wenn der Racer sich bisher noch nicht gedreht hat - fahre ein Stück rückwarts. Wenn der Racer sich gerade schon gedreht hat, drehe in dieselbe Richtung wie vorher.
|
|
if((abstand_links_cm < US_MIN_ABSTAND_LI_RE) && (abstand_rechts_cm < US_MIN_ABSTAND_LI_RE)){
|
|
if(anzahl_drehung == 0){
|
|
coderacer.rueckwaerts();
|
|
delay(300);
|
|
coderacer.anhalten();
|
|
//noch mal Abstand messen ...
|
|
coderacer.servo_links();
|
|
// Abstand messen und merken.
|
|
abstand_links_cm = coderacer.abstand_messen();
|
|
// Nach rechts schauen!
|
|
coderacer.servo_rechts();
|
|
// Abstand messen und merken.
|
|
abstand_rechts_cm = coderacer.abstand_messen();
|
|
}
|
|
if( anzahl_drehung > MAX_ANZAHL_DREHUNGEN ){
|
|
anzahl_drehung = 0;
|
|
} else {
|
|
anzahl_drehung ++;
|
|
}
|
|
}
|
|
|
|
// wenn der Racer sich noch nicht gedreht hat (anzahl_drehung == 0) - oder gerade rückwärts gefahren ist (anzahl_drehung == 1) drehe jetzt dahin wo mehr Platz ist;
|
|
if(anzahl_drehung <= 1){
|
|
// Welcher Abstand ist größer?
|
|
if(abstand_links_cm > abstand_rechts_cm){
|
|
// Links ist mehr Platz!
|
|
drehung = links;
|
|
}
|
|
else{
|
|
// Rechts ist mehr Platz!
|
|
drehung = rechts;
|
|
}
|
|
}
|
|
|
|
switch(drehung){
|
|
case links:
|
|
coderacer.links();
|
|
coderacer.servo_mitte();
|
|
break;
|
|
case rechts:
|
|
coderacer.links();
|
|
coderacer.servo_mitte();
|
|
break;
|
|
}
|
|
}
|
|
else{
|
|
// Ja! Die Bahn ist frei
|
|
// Wenn die Bahn richtig frei ist, dann können wir den Zähler fürs drehen auf 0 setzen ...
|
|
if( abstand_vorn_cm > (US_STOP_ABSTAND_CM + US_MIN_ABSTAND_LI_RE)){
|
|
anzahl_drehung = 0;
|
|
}
|
|
coderacer.vorwaerts();
|
|
}
|
|
|
|
} else {
|
|
anzahl_drehung = 0;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|