From 8575b1b1895eaf5ed537a833868b4e44150c78b5 Mon Sep 17 00:00:00 2001 From: jglueck Date: Wed, 15 Apr 2020 16:02:03 +0200 Subject: [PATCH] New version with advanced calibration --- .../lib/CodeRacer_MKII/CodeRacer_MKII.cpp | 19 +-- vsode/coderacer_mkII/src/coderacer_main.cpp | 156 +++++++++--------- 2 files changed, 89 insertions(+), 86 deletions(-) diff --git a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp index 9aea0f2..3dce5a4 100644 --- a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp +++ b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp @@ -1360,11 +1360,8 @@ void CodeRacerMKII::turn_degree(int degree){ * @return nothing */ void CodeRacerMKII::drive_ticks_left(unsigned int Ticks, bool drive = true ){ - if(left_step_stopcount<=left_step_counter) - { - left_step_stopcount = left_step_counter+ Ticks; - if(true == drive){drive_forward();} - } + left_step_stopcount = left_step_counter+ Ticks; + if(true == drive){drive_forward();} } /** @brief tells the coderacer to drive a specified number of ticks for the right wheel. * @param Ticks number of ticks @@ -1372,11 +1369,8 @@ void CodeRacerMKII::drive_ticks_left(unsigned int Ticks, bool drive = true ){ * @return nothing */ void CodeRacerMKII::drive_ticks_right(unsigned int Ticks, bool drive = true){ - if(right_step_stopcount<=right_step_counter) - { - right_step_stopcount = right_step_counter+ Ticks; - if(true == drive){drive_forward();} - } + right_step_stopcount = right_step_counter+ Ticks; + if(true == drive){drive_forward();} } /** @brief tells the coderacer to drive a specified distance in mm. requires two values for both the left and the right wheel. * @param leftdistance the distance to be driven by the left wheel @@ -1544,6 +1538,8 @@ static void IRAM_ATTR _count_steps_left() { if (left_step_counter==left_step_stopcount) { global_stop_driving(); + left_step_stopcount= 0; + right_step_stopcount=0; } } /** @brief this routine is called each time the left PE barrier changes its state. it increments (and by that counts) the ticks driven by the right wheel and is also instructed to stop the racer if the stopcount set by drive_ticks() is reached using the global_stop_driving() routine @@ -1556,6 +1552,9 @@ static void IRAM_ATTR _count_steps_right() { if (right_step_counter==right_step_stopcount) { global_stop_driving(); + left_step_stopcount=0; + right_step_stopcount=0; + } } /** @brief this routine is called each time the left infrared sensor changes its state. it sets the boolean variable obstacle_left_side to true/false and, if obstacle_stop is enabled, also stops the racer using global_stop_driving() diff --git a/vsode/coderacer_mkII/src/coderacer_main.cpp b/vsode/coderacer_mkII/src/coderacer_main.cpp index 31447c9..f26b53a 100644 --- a/vsode/coderacer_mkII/src/coderacer_main.cpp +++ b/vsode/coderacer_mkII/src/coderacer_main.cpp @@ -18,19 +18,23 @@ CodeRacerMKII coderacer; int aabstand[160]; void filladistance(); int adegree[160]; -int ispeed=0; +unsigned int ispeed=0; float fZeit= 0; float fSpeedminleft= 0; float fSpeedminright=0; float fSpeedmaxleft=0; -int iTicks=31; +float fSpeedmaxright=0; +int iTicks=101; float callibration_drive(unsigned int tickstogo, float calfactor); -int Emit_vmin(bool state); -float calculate_min_veliocity(bool side); unsigned int getcount_function(bool left_notright); void set_speed_function(bool left_notright, unsigned int speed); unsigned int get_inmin(bool left_notright); +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; //---- Hier startet der Code zum Einstellen aller wichtigen Dinge. Setup() wird einmal ausgeführt. ---- void setup() { @@ -65,22 +69,54 @@ void loop() wait_ms(1000); } + // below - mitteln 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); + 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 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(); + + 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); + + 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); + + wait_ms(1000); coderacer.set_inactive(); } @@ -90,13 +126,13 @@ void loop() 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 ); + //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 ); + //Serial.printf("Steps after driving: %u \n", steps_after_driving ); coderacer.stop_driving(); return pwm_in*1.1; } @@ -122,70 +158,38 @@ void set_speed_function(bool left_notright, unsigned int speed) -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; - } +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()){}; -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