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-15 16:02:03 +02:00
unsigned int ispeed = 0 ;
2020-04-14 16:42:30 +02:00
float fZeit = 0 ;
float fSpeedminleft = 0 ;
float fSpeedminright = 0 ;
float fSpeedmaxleft = 0 ;
2020-04-15 16:02:03 +02:00
float fSpeedmaxright = 0 ;
int iTicks = 101 ;
2020-04-09 16:35:40 +02:00
float callibration_drive ( unsigned int tickstogo , float calfactor ) ;
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-04-15 16:02:03 +02:00
void calculate_veliocity ( unsigned int inleft , unsigned int inright , float * vleft , float * vright ) ;
const bool LEFT = true ;
const bool RIGHT = false ;
const unsigned int IN_MAX = 255 ;
2020-04-15 00:05:45 +02:00
2020-03-25 08:55:28 +01:00
//---- Hier startet der Code zum Einstellen aller wichtigen Dinge. Setup() wird einmal ausgeführt. ----
2020-08-21 11:21:29 +02:00
void setup2 ( ) {
2020-03-25 08:55:28 +01:00
// 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-08-21 11:21:29 +02:00
void loop2 ( )
2020-04-09 11:14:14 +02:00
{
2020-08-21 11:21:29 +02:00
coderacer . drive_distance_mm ( 1000 , 1000 ) ;
2020-04-09 11:14:14 +02:00
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-08-21 11:21:29 +02:00
2020-04-15 16:02:03 +02:00
// below - mitteln
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 ) ;
2020-04-15 16:02:03 +02:00
wait_ms ( 1000 ) ;
calculate_veliocity ( in_min_left , in_min_right , & fSpeedminleft , & fSpeedminright ) ;
wait_ms ( 1000 ) ;
calculate_veliocity ( IN_MAX , IN_MAX , & fSpeedmaxleft , & fSpeedmaxright ) ;
Serial . printf ( " Left vmin:%f vmax:%f Right vmin:%f vmax: %f \n " , fSpeedminleft , fSpeedmaxleft , fSpeedminright , fSpeedmaxright ) ;
//above - mitteln
float nLeft = ( fSpeedmaxleft - fSpeedminleft ) / ( IN_MAX - in_min_left ) ;
float mLeft = fSpeedmaxleft - nLeft * IN_MAX ;
float nRight = ( fSpeedmaxright - fSpeedminright ) / ( IN_MAX - in_min_right ) ;
float mRight = fSpeedmaxright - nRight * IN_MAX ;
Serial . printf ( " nleft: %f mleft:%f nright:%f mright:%f \n " , nLeft , mLeft , nRight , mRight ) ;
wait_ms ( 5000 ) ;
unsigned int vracer = 35 ;
unsigned int inleft = ( vracer - mLeft ) / nLeft ;
unsigned int inright = ( vracer - mRight ) / nRight ;
Serial . printf ( " inleft: %u inright:%u \n " , inleft , inright ) ;
coderacer . speed_settings ( inleft , inright ) ;
unsigned int lticks = coderacer . show_left_stepcounter ( ) ;
unsigned int rticks = coderacer . show_right_stepcounter ( ) ;
coderacer . drive_ticks ( 100 , 100 ) ;
while ( coderacer . is_driving ( ) ) { }
rticks = coderacer . show_right_stepcounter ( ) - rticks ;
lticks = coderacer . show_left_stepcounter ( ) - lticks ;
Serial . printf ( " Cal. Steps left: %u right: %u \n " , lticks , rticks ) ;
wait_ms ( 1000 ) ;
2020-04-15 00:05:45 +02:00
2020-04-15 16:02:03 +02:00
coderacer . speed_settings ( inleft , inleft ) ;
lticks = coderacer . show_left_stepcounter ( ) ;
rticks = coderacer . show_right_stepcounter ( ) ;
coderacer . drive_ticks ( 100 , 100 ) ;
while ( coderacer . is_driving ( ) ) { }
rticks = coderacer . show_right_stepcounter ( ) - rticks ;
lticks = coderacer . show_left_stepcounter ( ) - lticks ;
Serial . printf ( " Uncal.Steps left: %u right: %u \n " , lticks , rticks ) ;
2020-04-15 00:05:45 +02:00
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 ) ;
2020-04-15 16:02:03 +02:00
//Serial.printf("Steps before driving: %u \n", steps_before_driving );
2020-04-15 00:05:45 +02:00
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 )
{
2020-04-15 16:02:03 +02:00
//Serial.printf("Steps after driving: %u \n", steps_after_driving );
2020-04-15 00:05:45 +02:00
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-15 16:02:03 +02:00
void calculate_veliocity ( unsigned int inleft , unsigned int inright , float * vleft , float * vright )
{
coderacer . speed_settings ( inleft , inright ) ;
coderacer . set_left_start_time ( ) ;
coderacer . set_right_start_time ( ) ;
unsigned int iStepsbefore_left = coderacer . show_left_stepcounter ( ) ;
unsigned int iStepsbefore_right = coderacer . show_right_stepcounter ( ) ;
coderacer . drive_ticks ( iTicks , iTicks ) ;
while ( coderacer . is_driving ( ) ) { } ;
unsigned int iStepsafter_left = coderacer . show_left_stepcounter ( ) ;
unsigned int iStepsafter_right = coderacer . show_right_stepcounter ( ) ;
unsigned int iDifferenz_left = iStepsafter_left - iStepsbefore_left - 1 ;
unsigned int iDifferenz_right = iStepsafter_right - iStepsbefore_right - 1 ;
Serial . printf ( " Schritte before left: %u right: %u Steps after left:%u right:%u Steps diff left: %u right %u \n " , iStepsbefore_left , iStepsbefore_right , iStepsafter_left , iStepsafter_right , iDifferenz_left , iDifferenz_right ) ;
unsigned long lzeit_left = coderacer . show_left_time_of_last_tick ( ) - coderacer . show_left_start_time ( ) ;
unsigned long lzeit_right = coderacer . show_right_time_of_last_tick ( ) - coderacer . show_right_start_time ( ) ;
//Serial.printf("Links Startzeit:%lu Endzeit:%lu Runtime:%lu\n", coderacer.show_left_start_time(),coderacer.show_left_time_of_last_tick(), lzeit_left);
//Serial.printf("Rechts Startzeit:%lu Endzeit:%lu Runtime:%lu\n", coderacer.show_right_start_time(), coderacer.show_right_time_of_last_tick(), lzeit_right);
float fSpeedleft = iDifferenz_left / ( ( float ) lzeit_left / 1000.0 ) ;
* vleft = fSpeedleft ;
float fSpeedright = iDifferenz_right / ( ( float ) lzeit_right / 1000.0 ) ;
* vright = fSpeedright ;
//Serial.printf("Speed left: %f right: %f\n" , fSpeedleft, fSpeedright);
2020-03-25 08:55:28 +01:00
}
2020-04-14 16:42:30 +02:00
2020-04-15 16:02:03 +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