#include "CodeRacer_MKII.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;
CodeRacerMKII coderacer;

int aabstand[160];
void filladistance();
int adegree[160];
int ispeed=1;
//---- Hier startet der Code zum Einstellen aller wichtigen Dinge. Setup() wird einmal ausgeführt. ----
void setup() {
    // Monitor
    Serial.begin(9600);               //  Serial Monitor aktivieren. Mit dem Monitor kann man sich Werte und Meldungen anzeigen lassen.

    // CodeRacer initialisieren
    coderacer.begin();

    coderacer.servo_set_to_left();
    delay(10);
    coderacer.servo_set_to_right();
    delay(10);
    coderacer.servo_set_to_center(); 
    
    anzahl_drehung = 0;
    drehung = links;
    coderacer.set_syncstop(true);
    coderacer.set_obstacle_stop(false);
    coderacer.speed_settings(135, 135);
}

//---- 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() 
{
 bool started = false;
  while(coderacer.start_stop()== 1)
  {
    if(false == started)
    {
      started = true;
      delay(1000);
    }

    unsigned int stepsprevl, stepsprevr ;
    for(unsigned int v= 80;v<=255;v=v+10)
    {
      coderacer.speed_settings(v, v);
      stepsprevl = coderacer.show_left_stepcounter();
      stepsprevr = coderacer.show_right_stepcounter();
      coderacer.drive_ticks_left(2000, true);
      while (1== coderacer.is_driving()){}
      Serial.printf("v=%i  links*(2000)=%i rechts=%i\n",v,  coderacer.show_left_stepcounter() - stepsprevl , coderacer.show_right_stepcounter() - stepsprevr);
    }
    

  }
}
//Serial.printf("links %i\n", coderacer.show_left_stepcounter());
//Serial.printf("rechts %i\n", coderacer.show_right_stepcounter());
//Serial.printf("%i\n", coderacer.show_distance_mm());