diff --git a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp index 11d9c66..d7e1f95 100644 --- a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp +++ b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp @@ -509,6 +509,30 @@ unsigned int CodeRacerMKII::show_right_stepcounter(){ return right_step_counter; } +unsigned long CodeRacerMKII::show_left_time_of_last_tick(){ + return timeafterleft; +} + +unsigned long CodeRacerMKII::show_right_time_of_last_tick(){ + return timeafterright; +} + +unsigned long CodeRacerMKII::show_left_start_time(){ + return timebeforeleft; +} + +unsigned long CodeRacerMKII::show_right_start_time(){ + return timebeforeright; +} + +void CodeRacerMKII::set_left_start_time(){ + timebeforeleft=0; +} + +void CodeRacerMKII::set_right_start_time(){ + timebeforeright=0; +} + /** @} */ // end of group higherlevelgetters /** @} */ // end of group higherlevel @@ -1490,6 +1514,8 @@ void global_stop_driving(){ * @return nothing */ void IRAM_ATTR CodeRacerMKII::_count_steps_left() { + timeafterleft= millis(); + timebeforeleft = timebeforeleft == 0 ? timeafterleft : timebeforeleft; left_step_counter++; if (left_step_counter==left_step_stopcount) { @@ -1501,6 +1527,8 @@ void IRAM_ATTR CodeRacerMKII::_count_steps_left() { * @return nothing */ void IRAM_ATTR CodeRacerMKII::_count_steps_right() { + timeafterright= millis(); + timebeforeright = timebeforeright == 0 ? timeafterright : timebeforeright; right_step_counter++; if (right_step_counter==right_step_stopcount) { diff --git a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.h b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.h index e715953..6246e29 100644 --- a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.h +++ b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.h @@ -89,6 +89,10 @@ static volatile unsigned int right_step_counter=0; static volatile unsigned int right_step_stopcount=0; static volatile bool syncstop= 0; static volatile bool _drive; +static volatile float timebeforeleft= 0; +static volatile float timebeforeright= 0; +static volatile float timeafterleft= 0; +static volatile float timeafterright= 0; static volatile bool obstacle_stop= false; static volatile bool obstacle_left_side= false; static volatile bool obstacle_right_side= false; @@ -233,6 +237,12 @@ class CodeRacerMKII { unsigned int show_distance_mm(); unsigned int show_left_stepcounter(); unsigned int show_right_stepcounter(); + unsigned long show_left_time_of_last_tick(); + unsigned long show_right_time_of_last_tick(); + unsigned long show_left_start_time(); + unsigned long show_right_start_time(); + void set_left_start_time(); + void set_right_start_time(); // higher level {code}racer services diff --git a/vsode/coderacer_mkII/src/coderacer_main.cpp b/vsode/coderacer_mkII/src/coderacer_main.cpp index 3998477..0bb94d6 100644 --- a/vsode/coderacer_mkII/src/coderacer_main.cpp +++ b/vsode/coderacer_mkII/src/coderacer_main.cpp @@ -18,8 +18,15 @@ CodeRacerMKII coderacer; int aabstand[160]; void filladistance(); int adegree[160]; -int ispeed=1; +int ispeed=0; +float fZeit= 0; +float fSpeedminleft= 0; +float fSpeedminright=0; +float fSpeedmaxleft=0; +int iTicks=31; float callibration_drive(unsigned int tickstogo, float calfactor); +int Emit_vmin(bool state); +float calculate_min_veliocity(bool side); //---- Hier startet der Code zum Einstellen aller wichtigen Dinge. Setup() wird einmal ausgeführt. ---- void setup() { // Monitor @@ -38,6 +45,7 @@ void setup() { drehung = links; coderacer.set_syncstop(true); coderacer.set_obstacle_stop(false); + } //---- 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 @@ -51,20 +59,78 @@ void loop() started = true; delay(1000); } - - float calf = callibration_drive(1000, 0.04); - Serial.println(calf); - float compare_calf = callibration_drive(1000, calf); - Serial.println(compare_calf); - } - + float vminright= calculate_min_veliocity(1); + Serial.println(vminright); - - - - - + delay(5000); + + } } + +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; + } + +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