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 ] ;
2020-04-14 16:42:30 +02:00
int ispeed = 0 ;
float fZeit = 0 ;
float fSpeedminleft = 0 ;
float fSpeedminright = 0 ;
float fSpeedmaxleft = 0 ;
int iTicks = 31 ;
2020-04-09 16:35:40 +02:00
float callibration_drive ( unsigned int tickstogo , float calfactor ) ;
2020-04-14 16:42:30 +02:00
int Emit_vmin ( bool state ) ;
float calculate_min_veliocity ( bool side ) ;
2020-04-15 00:05:45 +02:00
unsigned int getcount_function ( bool left_notright ) ;
void set_speed_function ( bool left_notright , unsigned int speed ) ;
unsigned int get_inmin ( bool left_notright ) ;
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-04-14 16:42:30 +02:00
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-04-15 00:05:45 +02:00
while ( coderacer . start_stop ( ) = = 1 )
2020-03-30 13:56:43 +02:00
{
2020-04-09 11:14:14 +02:00
if ( false = = started )
{
started = true ;
2020-04-15 00:05:45 +02:00
wait_ms ( 1000 ) ;
2020-04-09 11:14:14 +02:00
}
2020-04-15 00:05:45 +02:00
coderacer . stop_driving ( ) ;
unsigned int in_min_left = get_inmin ( true ) ;
wait_ms ( 1000 ) ; // do not use delay() this will create problems with interrupt routines! use the wait_ms() instead !!!!
unsigned int in_min_right = get_inmin ( false ) ;
Serial . printf ( " left in_min: %u right in_min: %u \n " , in_min_left , in_min_right ) ;
//float vminright= calculate_min_veliocity(1);
//Serial.println(vminright);
//speedleft = speedleft == 0 ? 128 : 0;
//speedright = speedright == 0 ? 128 : 0;
//coderacer.drive_forward(speedleft, speedright);
//coderacer.set_drives_speed_left_right(speedleft,speedright);
//coderacer.drive_forward();
//wait_ms(1000);
//coderacer.stop_driving();
wait_ms ( 1000 ) ;
coderacer . set_inactive ( ) ;
2020-04-14 16:42:30 +02:00
}
}
2020-04-09 11:14:14 +02:00
2020-04-15 00:05:45 +02:00
unsigned int get_inmin ( bool left_notright )
{
unsigned int steps_before_driving = getcount_function ( left_notright ) ;
Serial . printf ( " Steps before driving: %u \n " , steps_before_driving ) ;
for ( unsigned int pwm_in = 0 ; pwm_in < 255 ; pwm_in = pwm_in + 5 )
{
unsigned int steps_after_driving = getcount_function ( left_notright ) ;
if ( steps_after_driving > steps_before_driving )
{
Serial . printf ( " Steps after driving: %u \n " , steps_before_driving ) ;
coderacer . stop_driving ( ) ;
return pwm_in * 1.1 ;
}
set_speed_function ( left_notright , pwm_in ) ;
wait_ms ( 100 ) ;
}
coderacer . stop_driving ( ) ;
return 0 ;
}
unsigned int getcount_function ( bool left_notright )
{
if ( true = = left_notright ) { return coderacer . show_left_stepcounter ( ) ; }
else { return coderacer . show_right_stepcounter ( ) ; }
}
void set_speed_function ( bool left_notright , unsigned int speed )
{
if ( true = = left_notright ) { coderacer . drive_forward ( speed , 0 ) ; }
else { coderacer . drive_forward ( 0 , speed ) ; }
}
2020-04-14 16:42:30 +02:00
float calculate_min_veliocity ( bool side )
{
ispeed = Emit_vmin ( side ) ;
coderacer . speed_settings ( ispeed , ispeed ) ;
coderacer . set_left_start_time ( ) ;
coderacer . set_right_start_time ( ) ;
int iStepsbefore = coderacer . show_left_stepcounter ( ) ;
Serial . printf ( " Schritte: %i \n " , iStepsbefore ) ;
coderacer . drive_ticks ( iTicks , iTicks ) ;
while ( coderacer . is_driving ( ) ) { } ;
if ( 1 = = side )
{
fZeit = ( coderacer . show_left_time_of_last_tick ( ) - coderacer . show_left_start_time ( ) ) / 1000.0 ;
int iDifferenz = coderacer . show_left_stepcounter ( ) - iStepsbefore - 1 ;
Serial . printf ( " Zeit: %f timebefore: %lu timeafter: %lu Ticks: %i differenz: %i \n " , fZeit , coderacer . show_left_start_time ( ) , coderacer . show_left_time_of_last_tick ( ) , coderacer . show_left_stepcounter ( ) , iDifferenz ) ;
fSpeedminleft = ( float ) iDifferenz / fZeit ;
}
else
{
fZeit = ( coderacer . show_right_time_of_last_tick ( ) - coderacer . show_right_start_time ( ) ) / 1000.0 ;
int iDifferenz = coderacer . show_right_stepcounter ( ) - iStepsbefore - 1 ;
fSpeedminright = ( float ) iDifferenz / fZeit ;
}
if ( 1 = = side )
{
return fSpeedminleft ;
}
else return fSpeedminright ;
}
2020-04-09 16:35:40 +02:00
2020-04-14 16:42:30 +02:00
int Emit_vmin ( bool state ) {
int iSteps = 0 ;
if ( 1 = = state )
{
iSteps = coderacer . show_left_stepcounter ( ) ;
}
else { iSteps = coderacer . show_right_stepcounter ( ) ; }
for ( int i = 0 ; i < = 150 ; i + + )
{
if ( 1 = = state )
{
if ( iSteps < coderacer . show_left_stepcounter ( ) )
{
return i * 1.1 ;
break ;
}
coderacer . drive_forward ( i , 0 ) ;
Serial . println ( i ) ;
}
else
{
if ( iSteps < coderacer . show_right_stepcounter ( ) )
{
return i * 1.1 ;
break ;
}
coderacer . drive_forward ( 0 , i ) ;
Serial . println ( i ) ;
}
}
return 0 ;
2020-03-25 08:55:28 +01:00
}
2020-04-14 16:42:30 +02: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 ;
2020-04-09 16:36:25 +02:00
float GesamtSummeL = 0 ;
2020-04-09 16:35:40 +02:00
unsigned int runcounter = 0 ;
2020-04-14 16:42:30 +02:00
int runs = 4 ;
2020-04-09 16:40:45 +02:00
float MittelSummeR ;
2020-04-09 16:35:40 +02:00
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 ;
2020-04-14 16:42:30 +02:00
for ( int i = 1 ; i < = runs ; i + + )
2020-04-09 16:35:40 +02:00
{
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