251 lines
9.1 KiB
C++
251 lines
9.1 KiB
C++
#include "CodeRacer_MKII.h"
|
|
|
|
#include <ESP32Servo.h>
|
|
#include "esp32-hal-ledc.h"
|
|
|
|
//----- Werte für den Ultraschallsensor -----
|
|
#define US_STOP_ABSTAND_CM 20 // Wenn der gemessene Abstand kleiner ist, hält der CodeRacer an
|
|
#define US_MIN_ABSTAND_LI_RE 8 // Wenn der Unterschied zwischen linkem und und rechtem Abstand kleiner ist, dann drehe in dieselbe Richtugn wie vorher weiter
|
|
#define MAX_ANZAHL_DREHUNGEN 10 // Wenn der Coderacer sich schon so oft gedreht hat ohne eine Stelle zu finden, wo es Platz gibt - fahren wir mal ein Stück rückwärts ...
|
|
|
|
//----- Variablen, die wir brauchen um uns Werte zu merken ----
|
|
long abstand_vorn_cm, abstand_links_cm, abstand_rechts_cm;
|
|
enum drehrichtung {links=0, rechts};
|
|
drehrichtung drehung = links;
|
|
unsigned int anzahl_drehung = 0;
|
|
CodeRacerMKII coderacer;
|
|
|
|
int aabstand[160];
|
|
void filladistance();
|
|
int adegree[160];
|
|
unsigned int ispeed=0;
|
|
float fZeit= 0;
|
|
float fSpeedminleft= 0;
|
|
float fSpeedminright=0;
|
|
float fSpeedmaxleft=0;
|
|
float fSpeedmaxright=0;
|
|
int iTicks=101;
|
|
float callibration_drive(unsigned int tickstogo, float calfactor);
|
|
|
|
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 setup2() {
|
|
// Monitor
|
|
Serial.begin(9600); // Serial Monitor aktivieren. Mit dem Monitor kann man sich Werte und Meldungen anzeigen lassen.
|
|
|
|
// CodeRacer initialisieren
|
|
coderacer.begin();
|
|
|
|
coderacer.servo_set_to_left();
|
|
delay(10);
|
|
coderacer.servo_set_to_right();
|
|
delay(10);
|
|
coderacer.servo_set_to_center();
|
|
|
|
anzahl_drehung = 0;
|
|
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
|
|
void loop2()
|
|
{
|
|
coderacer.drive_distance_mm(1000, 1000);
|
|
|
|
bool started = false;
|
|
while(coderacer.start_stop()== 1)
|
|
{
|
|
if(false == started)
|
|
{
|
|
started = true;
|
|
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 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();
|
|
}
|
|
}
|
|
|
|
|
|
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 );
|
|
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_after_driving );
|
|
coderacer.stop_driving();
|
|
return pwm_in*1.1;
|
|
}
|
|
set_speed_function(left_notright, pwm_in);
|
|
wait_ms(100);
|
|
}
|
|
coderacer.stop_driving();
|
|
return 0 ;
|
|
}
|
|
|
|
unsigned int getcount_function(bool left_notright)
|
|
{
|
|
if(true == left_notright){return coderacer.show_left_stepcounter();}
|
|
else{return coderacer.show_right_stepcounter();}
|
|
}
|
|
|
|
void set_speed_function(bool left_notright, unsigned int speed)
|
|
{
|
|
if(true == left_notright){coderacer.drive_forward(speed,0);}
|
|
else{coderacer.drive_forward(0,speed);}
|
|
}
|
|
|
|
|
|
|
|
|
|
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()){};
|
|
|
|
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());
|
|
|
|
float callibration_drive(unsigned int tickstogo, float calfactor)
|
|
{
|
|
unsigned int stepsprevl;
|
|
unsigned int stepsprevr;
|
|
float GesamtSummeR = 0;
|
|
float GesamtSummeL=0;
|
|
unsigned int runcounter = 0;
|
|
int runs= 4;
|
|
float MittelSummeR;
|
|
float MittelSummeL;
|
|
unsigned int tickcheck= tickstogo*1.02;
|
|
for(unsigned int v= 190;v<=200;v=v+10)
|
|
{
|
|
unsigned int vr = v + calfactor*255;
|
|
unsigned int vl = v - calfactor*255;
|
|
runcounter ++;
|
|
coderacer.speed_settings(vl, vr);
|
|
Serial.println(vl);
|
|
Serial.println(vr);
|
|
int SummeR = 0;
|
|
int SummeL=0;
|
|
for (int i=1; i<=runs; i++)
|
|
{
|
|
stepsprevl = coderacer.show_left_stepcounter();
|
|
stepsprevr = coderacer.show_right_stepcounter();
|
|
coderacer.drive_ticks_left(tickstogo, true);
|
|
while (1== coderacer.is_driving()){}
|
|
delay(1000);
|
|
SummeR = SummeR + (coderacer.show_right_stepcounter() - stepsprevr);
|
|
SummeL= SummeL +(coderacer.show_left_stepcounter()- stepsprevl);
|
|
if (tickcheck<(coderacer.show_left_stepcounter() - stepsprevl))
|
|
{
|
|
Serial.printf("ERROR: calibration failed");
|
|
Serial.printf("links=%i\n" ,coderacer.show_left_stepcounter()- stepsprevl);
|
|
return 0;
|
|
}
|
|
Serial.printf("v=%i links=%i rechts=%i\n" ,v, coderacer.show_left_stepcounter()- stepsprevl, coderacer.show_right_stepcounter() - stepsprevr);
|
|
|
|
}
|
|
GesamtSummeR = GesamtSummeR+ SummeR/4.0;
|
|
GesamtSummeL = GesamtSummeL+ SummeL/4.0;
|
|
}
|
|
MittelSummeR= GesamtSummeR/(float)runcounter;
|
|
MittelSummeL= GesamtSummeL/(float)runcounter;
|
|
float CalibrationFactor= MittelSummeL/ MittelSummeR;
|
|
return CalibrationFactor;
|
|
}
|
|
|