#include <BluetoothSerial.h>
#include <ESP32Servo.h>

BluetoothSerial SerialBT;

Servo lenkungServo;

void setup() {
  //BT Connection starten
  SerialBT.begin("T1Xplorer22");
  
  //Den Servo für den ESP32 bereit machen
  lenkungServo.setPeriodHertz(50);
  lenkungServo.attach(14, 500, 2500);
  
  //Die beiden Pins für die Antriebsteuerung setzen
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);
}

void loop() {
  //Wenn an der BT Connection Daten anliegen
  if (SerialBT.available()) {
    String empfangen = "";
    //Die ankommenden Daten einlesen, bis alle da
    while (SerialBT.available()) {
      empfangen += (char)SerialBT.read();
      delay(3);
    }
    //der erste Char gibt auskunft über die Richtung des Motors
    String hilf = (String) empfangen.charAt(0);
    int richtung = hilf.toInt();
    /*
       0 = still
       1 = Vorwärts
       2 = Rückwärts
    */
    //Entsprechend der Definition der Daten die Pins schalten
    if (richtung == 0) {
      digitalWrite(12, LOW);
      digitalWrite(13, LOW);
    } else if (richtung == 1) {
      digitalWrite(12, HIGH);
      digitalWrite(13, LOW);
    } else if (richtung == 2) {
      digitalWrite(12, LOW);
      digitalWrite(13, HIGH);
    }


    //Die ersten beiden Char entfernen (<richtung> und <;>)
    empfangen.remove(0, 2);
    //Der übrige String gibt nun Auskunft über den Lenkwinkel
    int lenkung = empfangen.toInt();

    //Entsprechend der Definition der Daten die den Servo setzen
    if (lenkung == 50) {
      lenkungServo.write(90);
    } else if ((lenkung > 50)&&(lenkung<100)) {
      lenkungServo.write((map(lenkung, 51, 100, 89, 55)));
    } else if ((lenkung < 50)&&(lenkung>0)) {
      lenkungServo.write((map(lenkung, 0, 49, 125, 91)));
    }
  }
  delay(10);
}