experimental code for calibration
This commit is contained in:
parent
ce119f6390
commit
8cd52dd6ab
3 changed files with 119 additions and 14 deletions
|
@ -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)
|
||||
{
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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();
|
||||
|
|
Loading…
Reference in a new issue