coderacer/library
2
0
Fork 0
Code Issues Pull requests Projects Releases 2 Packages Wiki Activity

New version with advanced calibration

Browse source
This commit is contained in:
jglueck 2020-04-15 16:02:03 +02:00
parent 4dc43aa247
commit 8575b1b189
2 changed files with 89 additions and 86 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

19
vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp
View file

@ -1360,11 +1360,8 @@ void CodeRacerMKII::turn_degree(int degree){
* @return nothing * @return nothing
*/ */
void CodeRacerMKII::drive_ticks_left(unsigned int Ticks, bool drive = true ){ 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. /** @brief tells the coderacer to drive a specified number of ticks for the right wheel.
* @param Ticks number of ticks * @param Ticks number of ticks
@ -1372,11 +1369,8 @@ void CodeRacerMKII::drive_ticks_left(unsigned int Ticks, bool drive = true ){
* @return nothing * @return nothing
*/ */
void CodeRacerMKII::drive_ticks_right(unsigned int Ticks, bool drive = true){ 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. /** @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 * @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) if (left_step_counter==left_step_stopcount)
{ {
global_stop_driving(); 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 /** @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) if (right_step_counter==right_step_stopcount)
{ {
global_stop_driving(); 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() /** @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()

154
vsode/coderacer_mkII/src/coderacer_main.cpp
View file

@ -18,19 +18,23 @@ CodeRacerMKII coderacer;
int aabstand[160]; int aabstand[160];
void filladistance(); void filladistance();
int adegree[160]; int adegree[160];
int ispeed=0; unsigned int ispeed=0;
float fZeit= 0; float fZeit= 0;
float fSpeedminleft= 0; float fSpeedminleft= 0;
float fSpeedminright=0; float fSpeedminright=0;
float fSpeedmaxleft=0; float fSpeedmaxleft=0;
int iTicks=31; float fSpeedmaxright=0;
int iTicks=101;
float callibration_drive(unsigned int tickstogo, float calfactor); 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); unsigned int getcount_function(bool left_notright);
void set_speed_function(bool left_notright, unsigned int speed); void set_speed_function(bool left_notright, unsigned int speed);
unsigned int get_inmin(bool left_notright); 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. ---- //---- Hier startet der Code zum Einstellen aller wichtigen Dinge. Setup() wird einmal ausgeführt. ----
void setup() { void setup() {
@ -65,22 +69,54 @@ void loop()
wait_ms(1000); wait_ms(1000);
} }
// below - mitteln
coderacer.stop_driving(); coderacer.stop_driving();
unsigned int in_min_left = get_inmin(true); 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 !!!! 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); 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); 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 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);
//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); wait_ms(1000);
coderacer.set_inactive(); coderacer.set_inactive();
} }
@ -90,13 +126,13 @@ void loop()
unsigned int get_inmin(bool left_notright ) unsigned int get_inmin(bool left_notright )
{ {
unsigned int steps_before_driving = getcount_function(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) for(unsigned int pwm_in = 0; pwm_in < 255; pwm_in = pwm_in+5)
{ {
unsigned int steps_after_driving = getcount_function(left_notright); unsigned int steps_after_driving = getcount_function(left_notright);
if(steps_after_driving > steps_before_driving) 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(); coderacer.stop_driving();
return pwm_in*1.1; 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) void calculate_veliocity(unsigned int inleft, unsigned int inright, float* vleft, float* vright)
{ {
ispeed=Emit_vmin(side); coderacer.speed_settings(inleft, inright);
coderacer.speed_settings(ispeed, ispeed); coderacer.set_left_start_time();
coderacer.set_left_start_time(); coderacer.set_right_start_time();
coderacer.set_right_start_time(); unsigned int iStepsbefore_left = coderacer.show_left_stepcounter();
int iStepsbefore=coderacer.show_left_stepcounter(); unsigned int iStepsbefore_right = coderacer.show_right_stepcounter();
Serial.printf("Schritte: %i\n", iStepsbefore); coderacer.drive_ticks(iTicks, iTicks);
coderacer.drive_ticks(iTicks, iTicks); while(coderacer.is_driving()){};
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; unsigned int iStepsafter_left = coderacer.show_left_stepcounter();
} unsigned int iStepsafter_right = coderacer.show_right_stepcounter();
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){ unsigned int iDifferenz_left = iStepsafter_left - iStepsbefore_left-1;
int iSteps=0; unsigned int iDifferenz_right = iStepsafter_right - iStepsbefore_right-1;
if(1== state)
{ 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);
iSteps= coderacer.show_left_stepcounter();
} unsigned long lzeit_left = coderacer.show_left_time_of_last_tick()- coderacer.show_left_start_time();
else {iSteps= coderacer.show_right_stepcounter();} unsigned long lzeit_right = coderacer.show_right_time_of_last_tick()- coderacer.show_right_start_time();
for(int i=0;i<=150; i++)
{ //Serial.printf("Links Startzeit:%lu Endzeit:%lu Runtime:%lu\n", coderacer.show_left_start_time(),coderacer.show_left_time_of_last_tick(), lzeit_left);
if(1== state) //Serial.printf("Rechts Startzeit:%lu Endzeit:%lu Runtime:%lu\n", coderacer.show_right_start_time(), coderacer.show_right_time_of_last_tick(), lzeit_right);
{
if(iSteps<coderacer.show_left_stepcounter()) float fSpeedleft= iDifferenz_left/((float)lzeit_left/1000.0);
{ *vleft = fSpeedleft;
return i*1.1; float fSpeedright= iDifferenz_right/((float)lzeit_right/1000.0);
break; *vright = fSpeedright;
} //Serial.printf("Speed left: %f right: %f\n" , fSpeedleft, fSpeedright);
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;
} }
//Serial.printf("links %i\n", coderacer.show_left_stepcounter()); //Serial.printf("links %i\n", coderacer.show_left_stepcounter());
//Serial.printf("rechts %i\n", coderacer.show_right_stepcounter()); //Serial.printf("rechts %i\n", coderacer.show_right_stepcounter());
//Serial.printf("%i\n", coderacer.show_distance_mm()); //Serial.printf("%i\n", coderacer.show_distance_mm());