library/esp32_coderacer/esp32_coderacer.ino
2018-09-12 16:35:18 +02:00

128 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;
}
}