library/esp32_coderacer/esp32_coderacer.ino

#include <ESP32Servo.h>
#include "esp32-hal-ledc.h"

//----- Taster ------------
#define TASTERPIN 17

//----- Werte für den Servo -----
#define SERVOPIN 16               // Pin an dem der Servomotor angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
#define SERVO_45GRAD_LINKS 140    // Wert um den Servo 45 Grad nach links zu drehen ... der kann je nach Servo anders sein
#define SERVO_45GRAD_RECHTS 35    // Wert um den Servo 45 Grad nach rechtss zu drehen ... der kann je nach Servo anders sein
#define SERVO_0GRAD_MITTE 90      // Wert um den Servo in die Mitte zu drehen ... der kann je nach Servo anders sein
#define SERVO_10GRAD_MITTE 135
#define SERVO_M10GRAD_MITTE 40
#define SERVO_SCHWENK_LI -5
#define SERVO_SCHWENK_RE  5
#define SERVO_SCHWENK_MS 15

//----- Werte für den Ultraschallsensor -----
#define US_TRIG 12                // Pin an dem der TRIG Pin des Ultraschallsensor angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
#define US_ECHO 14                // Pin an dem der ECHO Pin des Ultraschallsensor angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
#define US_STOP_ABSTAND_CM 15     // Wenn der gemessene Abstand kleiner ist, hält der CodeRacer an

//----- Werte für die Motoren -----
#define MOTORRE_MAX_TEMPO 255         // Geschwindigkeit Motor1 ... ein Wert zwischen 0 und 255
#define MOTORRE_TEMPO 205         // Geschwindigkeit Motor1 ... ein Wert zwischen 0 und 255
#define MOTORLI_MAX_TEMPO 255         // Geschwindigkeit Motor1 ... ein Wert zwischen 0 und 255
#define MOTORLI_TEMPO 200         // Geschwindigkeit Motor1 ... ein Wert zwischen 0 und 255


#define MOTORRE_SPEED 2           // Pin an dem der SPEED/ENABLE Pin des rechten Motors angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
#define MOTORRE_FWRD 4            // Pin an dem der FORWÄRTS Pin des rechten Motors angeschlossen ist. Was vorwärts und rückwärts ist, muss probiert und vielleicht umgesteckt werden.'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
#define MOTORRE_BACK 15           // Pin an dem der RÜCKWÄRTS Pin des rechten Motors angeschlossen ist. Was vorwärts und rückwärts ist, muss probiert und vielleicht umgesteckt werden.'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
#define MOTORLI_SPEED 21          // Pin an dem der SPEED/ENABLE Pin des linken Motors angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
#define MOTORLI_FWRD 22           // Pin an dem der FORWÄRTS Pin des linken Motors angeschlossen ist. Was vorwärts und rückwärts ist, muss probiert und vielleicht umgesteckt werden.'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
#define MOTORLI_BACK 23            // Pin an dem der RÜCKWÄRTS Pin des linken Motors angeschlossen ist. Was vorwärts und rückwärts ist, muss probiert und vielleicht umgesteckt werden.'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
#define RACER_LINKS_MS 200        // Die Zeit in Millisekunden, die der Racer braucht um sich 45 Grad nach links zu drehen
#define RACER_RECHTS_MS 200       // Die Zeit in Millisekunden, die der Racer braucht um sich 45 Grad nach rechts zu drehen
#define MOTORPWM_LINKS 5          // PWM-Kanal für linken Motor
#define MOTORPWM_RECHTS 6         // PWM-Kanal für rechten Motor

//----- Werte für die LEDs -----
#define LED_VORWAERTS 26          // Pin an dem die VORWÄRTS LED angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
#define LED_STOP 25               // Pin an dem die STOP LED angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
#define LED_LINKS 27              // Pin an dem die LINKS LED angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
#define LED_RECHTS 33             // Pin an dem die RECHTS LED angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.

//----- Variablen, die wir brauchen um uns Werte zu merken ----
long abstand_vorn_cm, abstand_links_cm, abstand_rechts_cm;

int schwenkPosition = SERVO_0GRAD_MITTE;
int schwenkRichtung = SERVO_SCHWENK_LI;
long schwenkMillis = millis();

bool coderracer_activ = false;

//----- Objekte die wir haben. z.B. den Servo ----
Servo dummy_myservo;          // ein Servo-Objekt anlegen, um den Servo Motor steuern zu können (Timer 0 überspringen)
Servo myservo;          // ein Servo-Objekt anlegen, um den Servo Motor steuern zu können 

//---- 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.

    // Taster
    pinMode(TASTERPIN,INPUT_PULLUP);

    // Ultraschallsensor
    pinMode(US_TRIG, OUTPUT);           // Ultraschallsensor: TRIG ist ein Ausgangspin. Es wird ein Signal zum Ultraschallsensor gesendet
    pinMode(US_ECHO, INPUT);            // Ultraschallsensor: ECHO ist ein Eingangspin. Es wird ein Signal vom Ultraschallsensor empfangen

    // Servo
    myservo.attach(SERVOPIN);           // dem Servo Objekt "sagen" an welchen Pin am Schaltkreis der Server angeschlossen ist
   
    // Linker Motor
    pinMode(MOTORLI_FWRD,OUTPUT);       // Linker Motor FORWARD ist ein Ausgang.
    pinMode(MOTORLI_BACK,OUTPUT);       // Linker Motor BACKWARD ist ein Ausgang.
    ledcSetup(MOTORPWM_LINKS, 5000, 8);              // channel 1, 50 Hz, 8-bit width
    ledcAttachPin(MOTORLI_SPEED, MOTORPWM_LINKS);    // Linker Motor SPEED mit Kanal 1 verbunden
    analogWrite(MOTORLI_SPEED, 0);      // Linken Motor sicherheitshalber ausschalten :-) 

    // Rechter Motor
    pinMode(MOTORRE_FWRD,OUTPUT);       // Rechter Motor FORWARD ist ein Ausgang.
    pinMode(MOTORRE_BACK,OUTPUT);       // Rechter Motor BACKWARD ist ein Ausgang.
    ledcSetup(MOTORPWM_RECHTS, 5000, 8);              // channel 2, 50 Hz, 8-bit width
    ledcAttachPin(MOTORRE_SPEED, MOTORPWM_RECHTS);    // Rechter Motor SPEED mit Kanal 2 verbunden
    analogWrite(MOTORRE_SPEED, 0);      // Rechten Motor sicherheitshalber ausschalten :-) 

    pinMode(MOTORRE_SPEED,OUTPUT);      // Rechter Motor SPEED ist ein Ausgang.
    digitalWrite(MOTORRE_SPEED, 0);     // Rechten Motor sicherheitshalber ausschalten :-) 

    // LEDs
    pinMode(LED_VORWAERTS, OUTPUT);     // LED Vorwärts ist ein Ausgang
    pinMode(LED_STOP, OUTPUT);          // LED Stop ist ein Ausgang
    pinMode(LED_LINKS, OUTPUT);         // LED Links ist ein Ausgang
    pinMode(LED_RECHTS, OUTPUT);        // LED Rechts ist ein Ausgang

    // alle LEDS aus
    digitalWrite(LED_VORWAERTS, LOW);
    digitalWrite(LED_STOP, LOW);
    digitalWrite(LED_LINKS, LOW);
    digitalWrite(LED_RECHTS, LOW);

    // Servo in die Mitte Stellen
    ServoMitte(); 
}

//---- 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() {

  // Taster abfragen 
  RacerStartStop();
  if(true == coderracer_activ){
  
    // Abstand messen -> nach vorn
    abstand_vorn_cm = AbstandMessen();            
    // Zum anfahren muss das Tempo höher sein - also jetzt wieder Tempo runter ...
    RacerNormalTempo();
    // Abstandssensor schon verstellen ... dann hat er das bis zur nächsten Messung auch geschafft
    ServoSchwenk();
  
    // Ist die Bahn frei? 
    if(abstand_vorn_cm < US_STOP_ABSTAND_CM){     
      // Nein! Der Abstand nach vorn ist kleiner als erlaubt!
      // Racer anhalten
      RacerAnhalten();
      // Nach links schauen!
      ServoLinks();
      // Abstand messen und merken.
      abstand_links_cm = AbstandMessen();
      // Nach rechts schauen!
      ServoRechts();
      // Abstand messen und merken.
      abstand_rechts_cm = AbstandMessen();
      
      // Welcher Abstand ist größer?
      if(abstand_links_cm > abstand_rechts_cm){
        // Links ist mehr Platz!
        RacerLinks();
        // Servo in die Mitte Stellen
        ServoMitte(); 
      }
      else{
        // Rechts ist mehr Platz!
        RacerRechts();
        // Servo in die Mitte Stellen
        ServoMitte(); 
      }
    }
    else{
      // Ja! Die Bahn ist frei
      RacerVorwaerts();
    }

  }
  
}

//-------------- Funktionen und Prozeduren -------------------------

void RacerStartStop(void){
  if(digitalRead(TASTERPIN) == LOW){
    if(false == coderracer_activ){
      coderracer_activ = true;
      digitalWrite(LED_STOP, LOW);
    } else {
      coderracer_activ = false;
      digitalWrite(LED_STOP, HIGH);
    }
    delay(100);    // Taster prellen ...
    while(digitalRead(TASTERPIN) == LOW){
      // do nothing just wait ...
      delay(200);
    }
  }
  if(false == coderracer_activ){
    RacerAnhalten();
    ServoMitte(); 
    digitalWrite(LED_STOP, HIGH);
  } else {
    digitalWrite(LED_STOP, LOW);
  }
}

void RacerAnhalten(void){
  Serial.println("RACER_ANHALTEN");                            // Meldung am Monitor ausgeben
  // Rechten Motor abschalten 
  digitalWrite(MOTORRE_FWRD, LOW);  
  digitalWrite(MOTORRE_BACK, LOW);
  // Linken Motor abschalten 
  digitalWrite(MOTORLI_FWRD, LOW);  
  digitalWrite(MOTORLI_BACK, LOW);
  // Motoren beide ausschalten 
  analogWrite(MOTORRE_SPEED, 0);
  analogWrite(MOTORLI_SPEED, 0);
  // LEDs setzen
  digitalWrite(LED_VORWAERTS, LOW);
  digitalWrite(LED_STOP, HIGH);
  digitalWrite(LED_LINKS, LOW);
  digitalWrite(LED_RECHTS, LOW); 
  delay(1000); 
}

void RacerNormalTempo(void){
  Serial.println("RACER_NORMAL_TEMPO");                            // Meldung am Monitor ausgeben
  analogWrite(MOTORRE_SPEED, MOTORRE_TEMPO);
  analogWrite(MOTORLI_SPEED, MOTORLI_TEMPO);
}

void RacerVorwaerts(void){
  Serial.println("RACER_VORWAERTS");                            // Meldung am Monitor ausgeben
  // Rechten Motor auf vorwärts stellen 
  digitalWrite(MOTORRE_FWRD, HIGH);  
  digitalWrite(MOTORRE_BACK, LOW);
  // Linken Motor auf vorwärts stellen 
  digitalWrite(MOTORLI_FWRD, HIGH);  
  digitalWrite(MOTORLI_BACK, LOW);
  // Motoren beide anschalten 
  analogWrite(MOTORRE_SPEED, MOTORRE_MAX_TEMPO);
  analogWrite(MOTORLI_SPEED, MOTORLI_MAX_TEMPO);
  // LEDs setzen
  digitalWrite(LED_VORWAERTS, HIGH);
  digitalWrite(LED_STOP, LOW);
  digitalWrite(LED_LINKS, LOW);
  digitalWrite(LED_RECHTS, LOW);  
}

void RacerLinks(void){
  Serial.println("RACER_LINKS");                            // Meldung am Monitor ausgeben
  // LEDs setzen
  digitalWrite(LED_VORWAERTS, LOW);
  digitalWrite(LED_STOP, LOW);
  digitalWrite(LED_LINKS, HIGH);
  digitalWrite(LED_RECHTS, LOW);  
  // Rechten Motor auf vorwärts stellen 
  digitalWrite(MOTORRE_FWRD, HIGH);  
  digitalWrite(MOTORRE_BACK, LOW);
  // Linken Motor auf rückwärts stellen 
  digitalWrite(MOTORLI_FWRD, LOW);  
  digitalWrite(MOTORLI_BACK, HIGH);
  // Motoren beide anschalten 
  analogWrite(MOTORRE_SPEED, MOTORRE_MAX_TEMPO);
  analogWrite(MOTORLI_SPEED, MOTORLI_MAX_TEMPO);
  // Warten bis der RAcer sich gedreht hat
  delay(RACER_LINKS_MS);
  // Motoren wieder auschalten
  analogWrite(MOTORRE_SPEED, 0);
  analogWrite(MOTORLI_SPEED, 0);
}

void RacerRechts(void){
  Serial.println("RACER_RECHTS");                            // Meldung am Monitor ausgeben
    // LEDs setzen
  digitalWrite(LED_VORWAERTS, LOW);
  digitalWrite(LED_STOP, LOW);
  digitalWrite(LED_LINKS, LOW);
  digitalWrite(LED_RECHTS, HIGH);  
  // Rechten Motor auf rückwärts stellen 
  digitalWrite(MOTORRE_FWRD, LOW);  
  digitalWrite(MOTORRE_BACK, HIGH);
  // Linken Motor auf vorwärts stellen 
  digitalWrite(MOTORLI_FWRD, HIGH);  
  digitalWrite(MOTORLI_BACK, LOW);
  // Motoren beide anschalten 
  analogWrite(MOTORRE_SPEED, MOTORRE_MAX_TEMPO);
  analogWrite(MOTORLI_SPEED, MOTORLI_MAX_TEMPO);
  // Warten bis der RAcer sich gedreht hat
  delay(RACER_RECHTS_MS);
  // Motoren wieder auschalten
  analogWrite(MOTORRE_SPEED, 0);
  analogWrite(MOTORLI_SPEED, 0); 
}

void ServoRechts(void){
  Serial.println("SERVO_RECHTS");                              // Meldung am Monitor ausgeben
  myservo.write(SERVO_45GRAD_RECHTS);                          // Servo auf den Winkel rechts drehen
  delay(1000);                                                 // Kurz warten, dass der Servo die Stellung erreicht
}

void ServoLinks(void){
  Serial.println("SERVO_LINKS");                               // Meldung am Monitor ausgeben
  myservo.write(SERVO_45GRAD_LINKS);                           // Servo auf den Winkel links drehen
  delay(1000);                                                 // Kurz warten, dass der Servo die Stellung erreicht
}

void ServoMitte(void){
  Serial.println("SERVO_MITTE");                               // Meldung am Monitor ausgeben
  myservo.write(SERVO_0GRAD_MITTE);                            // Servo auf den Winkel links drehen
  schwenkPosition = SERVO_0GRAD_MITTE;                         // Servo schaut direkt nach vorn
  schwenkMillis = millis();
  delay(1000);                                                 // Kurz warten, dass der Servo die Stellung erreicht
}

void ServoSchwenk(void){
  if(millis() - schwenkMillis > SERVO_SCHWENK_MS){
    Serial.println("SERVO_SCHWENK");                               // Meldung am Monitor ausgeben
    schwenkPosition = schwenkPosition + schwenkRichtung;
    if(schwenkPosition <= SERVO_M10GRAD_MITTE){
      schwenkPosition = SERVO_M10GRAD_MITTE;
      schwenkRichtung = SERVO_SCHWENK_RE;
    }
    if(schwenkPosition >= SERVO_10GRAD_MITTE){
      schwenkPosition = SERVO_10GRAD_MITTE;
      schwenkRichtung = SERVO_SCHWENK_LI;
   }
    myservo.write(schwenkPosition);                            // Servo auf den Winkel links drehen
    schwenkMillis = millis();
  }
}


long AbstandMessen(){
  long abstand_cm,echo_dauer;
  // Messung starten - ein kurzer Pulse "HIGH" wird zum TRIG pin des Ultraschallsensors geschickt
  digitalWrite(US_TRIG,LOW);
  delayMicroseconds(2);
  digitalWrite(US_TRIG,HIGH);
  delayMicroseconds(10);
  digitalWrite(US_TRIG,LOW);

  // Messung der Dauer in Mikrosekundenmeasure bis das ECHO Pin vom Ultraschallsensor HIGH wird 
  pinMode(US_ECHO,INPUT);
  echo_dauer = pulseIn(US_ECHO,HIGH);

  // convert into cm ... 344m/sec is the speed of noise - thus 34400cm/sec ... or 34,400cm/milisec ... or 0,0344cm/microsec
  // the echo has to go the distance twice - forth and back - so the duration has to be the half of the measured one
  // distance_cm = echo_duration/2 * 0,0344   or   distance_cm = echo_duration/2 / 29,1 or distance_cm = echo_duration * 0,0172
  // distance_cm = (echo_duration/2) / 29.1;
  abstand_cm = echo_dauer * 0.0172;

  //Messwert am Monitor anzeigen
  Serial.print("ABSTAND_MESSEN. Der Abstand in cm ist:");
  Serial.println(abstand_cm);
  
  return(abstand_cm);
}

void analogWrite(uint8_t pin, uint8_t speed){
    if(pin == MOTORLI_SPEED){ledcWrite(MOTORPWM_LINKS, speed);} 
    if(pin == MOTORRE_SPEED){ledcWrite(MOTORPWM_RECHTS, speed);}
}