experimental code for calibration

This commit is contained in:
jglueck 2020-04-14 16:42:30 +02:00
parent ce119f6390
commit 8cd52dd6ab
3 changed files with 119 additions and 14 deletions

View file

@ -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)
{

View file

@ -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

View file

@ -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<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;
}
//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());
@ -76,6 +142,7 @@ float callibration_drive(unsigned int tickstogo, float calfactor)
float GesamtSummeR = 0;
float GesamtSummeL=0;
unsigned int runcounter = 0;
int runs= 4;
float MittelSummeR;
float MittelSummeL;
unsigned int tickcheck= tickstogo*1.02;
@ -89,7 +156,7 @@ float callibration_drive(unsigned int tickstogo, float calfactor)
Serial.println(vr);
int SummeR = 0;
int SummeL=0;
for (int i=1; i<=4; i++)
for (int i=1; i<=runs; i++)
{
stepsprevl = coderacer.show_left_stepcounter();
stepsprevr = coderacer.show_right_stepcounter();