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

New version with advanced calibration

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
*/
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()

156
vsode/coderacer_mkII/src/coderacer_main.cpp
View File

@ -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<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;
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);
}
//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());