library/vsode/coderacer_mkII/src/coderacer_main.cpp

241 lines
7.6 KiB
C++
Raw Normal View History

#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];
2020-04-14 16:42:30 +02:00
int ispeed=0;
float fZeit= 0;
float fSpeedminleft= 0;
float fSpeedminright=0;
float fSpeedmaxleft=0;
int iTicks=31;
2020-04-09 16:35:40 +02:00
float callibration_drive(unsigned int tickstogo, float calfactor);
2020-04-14 16:42:30 +02:00
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);
//---- Hier startet der Code zum Einstellen aller wichtigen Dinge. Setup() wird einmal ausgeführt. ----
void setup() {
// Monitor
2020-03-30 13:56:43 +02:00
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);
2020-04-14 16:42:30 +02:00
}
//---- 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 loop()
{
bool started = false;
while(coderacer.start_stop()== 1)
2020-03-30 13:56:43 +02:00
{
if(false == started)
{
started = true;
wait_ms(1000);
}
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);
//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);
coderacer.set_inactive();
2020-04-14 16:42:30 +02:00
}
}
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_before_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);}
}
2020-04-14 16:42:30 +02:00
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;
}
2020-04-09 16:35:40 +02:00
2020-04-14 16:42:30 +02:00
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;
}
2020-04-14 16:42:30 +02:00
//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());
2020-04-09 16:35:40 +02:00
float callibration_drive(unsigned int tickstogo, float calfactor)
{
unsigned int stepsprevl;
unsigned int stepsprevr;
float GesamtSummeR = 0;
2020-04-09 16:36:25 +02:00
float GesamtSummeL=0;
2020-04-09 16:35:40 +02:00
unsigned int runcounter = 0;
2020-04-14 16:42:30 +02:00
int runs= 4;
2020-04-09 16:40:45 +02:00
float MittelSummeR;
2020-04-09 16:35:40 +02:00
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;
2020-04-14 16:42:30 +02:00
for (int i=1; i<=runs; i++)
2020-04-09 16:35:40 +02:00
{
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;
}