2020-03-25 08:55:28 +01:00
# 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 ;
2020-04-09 11:14:14 +02:00
int aabstand [ 160 ] ;
void filladistance ( ) ;
int adegree [ 160 ] ;
int ispeed = 1 ;
2020-04-09 16:35:40 +02:00
float callibration_drive ( unsigned int tickstogo , float calfactor ) ;
2020-03-25 08:55:28 +01:00
//---- Hier startet der Code zum Einstellen aller wichtigen Dinge. Setup() wird einmal ausgeführt. ----
void setup ( ) {
// Monitor
2020-03-30 13:56:43 +02:00
Serial . begin ( 9600 ) ; // Serial Monitor aktivieren. Mit dem Monitor kann man sich Werte und Meldungen anzeigen lassen.
2020-03-25 08:55:28 +01:00
// 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 ;
2020-04-03 12:36:16 +02:00
coderacer . set_syncstop ( true ) ;
2020-04-09 11:14:14 +02:00
coderacer . set_obstacle_stop ( false ) ;
2020-03-25 08:55:28 +01:00
}
//---- 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
2020-04-09 11:14:14 +02:00
void loop ( )
{
bool started = false ;
2020-03-30 13:56:43 +02:00
while ( coderacer . start_stop ( ) = = 1 )
{
2020-04-09 11:14:14 +02:00
if ( false = = started )
{
started = true ;
delay ( 1000 ) ;
}
2020-04-09 16:35:40 +02:00
float calf = callibration_drive ( 1000 , 0.04 ) ;
Serial . println ( calf ) ;
float compare_calf = callibration_drive ( 1000 , calf ) ;
Serial . println ( compare_calf ) ;
}
2020-04-09 11:14:14 +02:00
2020-04-09 16:35:40 +02:00
2020-03-25 08:55:28 +01:00
}
2020-04-09 11:14:14 +02:00
//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());
2020-04-09 16:35:40 +02:00
float callibration_drive ( unsigned int tickstogo , float calfactor )
{
unsigned int stepsprevl ;
unsigned int stepsprevr ;
float GesamtSummeR = 0 ;
float GesamtSummeL = 0 ;
unsigned int runcounter = 0 ;
float MittelSummeR ;
float MittelSummeL ;
unsigned int tickcheck = tickstogo * 1.02 ;
for ( unsigned int v = 190 ; v < = 200 ; v = v + 10 )
{
unsigned int vr = v + calfactor * 255 ;
unsigned int vl = v - calfactor * 255 ;
runcounter + + ;
coderacer . speed_settings ( vl , vr ) ;
Serial . println ( vl ) ;
Serial . println ( vr ) ;
int SummeR = 0 ;
int SummeL = 0 ;
for ( int i = 1 ; i < = 4 ; i + + )
{
stepsprevl = coderacer . show_left_stepcounter ( ) ;
stepsprevr = coderacer . show_right_stepcounter ( ) ;
coderacer . drive_ticks_left ( tickstogo , true ) ;
while ( 1 = = coderacer . is_driving ( ) ) { }
delay ( 1000 ) ;
SummeR = SummeR + ( coderacer . show_right_stepcounter ( ) - stepsprevr ) ;
SummeL = SummeL + ( coderacer . show_left_stepcounter ( ) - stepsprevl ) ;
if ( tickcheck < ( coderacer . show_left_stepcounter ( ) - stepsprevl ) )
{
Serial . printf ( " ERROR: calibration failed " ) ;
Serial . printf ( " links=%i \n " , coderacer . show_left_stepcounter ( ) - stepsprevl ) ;
return 0 ;
}
Serial . printf ( " v=%i links=%i rechts=%i \n " , v , coderacer . show_left_stepcounter ( ) - stepsprevl , coderacer . show_right_stepcounter ( ) - stepsprevr ) ;
}
GesamtSummeR = GesamtSummeR + SummeR / 4.0 ;
GesamtSummeL = GesamtSummeL + SummeL / 4.0 ;
}
MittelSummeR = GesamtSummeR / ( float ) runcounter ;
MittelSummeL = GesamtSummeL / ( float ) runcounter ;
float CalibrationFactor = MittelSummeL / MittelSummeR ;
return CalibrationFactor ;
}
2020-03-25 08:55:28 +01:00