Hlimi Omar
TRR2018 omar
Diff: stateMachines.cpp
- Revision:
- 36:bccddd02966a
- Parent:
- 35:f202f7e4237a
- Child:
- 37:810cdbcbbf3f
--- a/stateMachines.cpp Tue Sep 18 21:05:29 2018 +0000
+++ b/stateMachines.cpp Wed Sep 19 07:43:16 2018 +0000
@@ -1,7 +1,7 @@
#include "stateMachines.h"
-#if DEBUG >0
+//#if DEBUG >0
Serial pc(USBTX, USBRX); // tx, rx
-#endif
+//#endif
//*************** declarations ***************
@@ -49,6 +49,7 @@
PwmOut PwmDirection(PB_5); // PWM3 ch2 TIM3
int pulseDirection_us = DIRECTION_PULSE_MIDDLE;
+double pulseDirection_us_temp ;
int pulseSpeed_us = INITAL_PULSE_SPEED_US;
//Capteurs direction
AnalogIn anaG90(CAPT_90_GAUCHE);//capteur ir coté gauche
@@ -64,13 +65,14 @@
#endif
//piste
-double largeurPiste = 150.0;
+double largeurPiste90 = 150.0;
+double largeurPiste45 = 150.0;
double positionSurPiste90 = 75.0;
double positionSurPiste90Prev = positionSurPiste90;
double positionSurPiste45 = 75.0;
double positionSurPiste45Prev = positionSurPiste45;
-int derive45,derive90;
+double derive45,derive90;
int lastDifferences90[NB_INTEGRAL_SAMPLES] = {0};//for integral correction
int lastDifferenceIndex = 0;
int integralSum;
@@ -93,9 +95,10 @@
//SPEED
double maxSpeed_cmps = 0;
-double tachySpeed_cmps = 0;
-double tachyStepsRegister = 0;
+double tachySpeed_cmps = 0; //en cm/s
+//double tachyStepsRegister = 0;
double tachySectionDist_cm = 0;
+double tachyTotalDist_cm = 0.0;
#ifdef FREINAGE_ADAPTATIF
Timer brakingTimer;
@@ -163,7 +166,7 @@
history[indexSample].time = timeSinceStart.read_us() ;
history[indexSample].position45 = positionSurPiste45;
history[indexSample].position90 = positionSurPiste90;
- history[indexSample].largeurPiste = largeurPiste;
+ history[indexSample].largeurPiste = largeurPiste90;
history[indexSample].pwm_thro_us = pulseSpeed_us;
history[indexSample].pwm_dir_us = pulseDirection_us;
history[indexSample].dist = tachySectionDist_cm,
@@ -171,8 +174,8 @@
history[indexSample].strLidar = strengthLidar;
indexSample++;
#if DEBUG > 0
- pc.printf("\r\nodo:%d dist = %d \tstrength = %.4ld \tC45D: %.4lf C45G: %.4lf C90D: %.4lf C90G: %.4lf looptime: %d micros",tachySectionDist_cm,distLidar,strengthLidar,distMurD45Moy,distMurG45Moy,distMurD90Moy,distMurG90Moy,timeSinceStart.read_us());// output signal strength value
- pc.printf("\r\nstate Murs: %d, state Section %d, state MaxSpeed %d, state Throttle %d\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro);
+ pc.printf("\r\nodo:%d dist = %.4lf \tstrength = %.4ld \tC45D: %.4lf C45G: %.4lf C90D: %.4lf C90G: %.4lf looptime: %.4lf micros",tachySectionDist_cm,distLidar,strengthLidar,distMurD45Moy,distMurG45Moy,distMurD90Moy,distMurG90Moy,timeSinceStart.read_us());// output signal strength value
+ pc.printf("\r\nstate Murs: %.4lf, state Section %.4lf, state MaxSpeed %.4lf, state Throttle %.4lf\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro);
//wait(2);
timeSinceStart.reset();
timeSinceStart.start();
@@ -231,20 +234,7 @@
void getTachySpeed()
{
- //tachySteps = VALEUR_DU_PIN;
- //poour gérer les vitesses lentes
- if(tachyStepsRegister == 0 && timerSinceGetTachy.read_us() < 500000) {
- return;//on attend encore un peu l'aquisition de la vitesse
- }
- tachySectionDist_cm += tachyStepsRegister;
- tachySpeed_cmps = (tachyStepsRegister * 1000000)/timerSinceGetTachy.read_us();
-#if DEBUG > 2
- pc.printf("IT: distance parcourue %d , vitesse:%d \r\n",tachySectionDist_cm,tachySpeed_cmps);
-#endif
- tachyStepsRegister=0.0;
- timerSinceGetTachy.reset();
- timerSinceGetTachy.start();
- return;
+
}
@@ -265,7 +255,7 @@
{
tab[index_fifo_ir] = 3.3 * (double)p->read(); // on convertit directement en volts
//tension proportionelle à l'inverse de la distance
- tab[index_fifo_ir] = 1.0/(0.0161 * tab[index_fifo_ir]);
+ tab[index_fifo_ir] = 1.0/(0.0161 * (double)tab[index_fifo_ir]);
int sumMoy = 0;
for(int k=0; k<size; k++) {
sumMoy+=tab[k];
@@ -275,7 +265,20 @@
void it4cm()
{
- tachyStepsRegister += TACHY_CM;
+ //faire un flag sur l'IT pour le freinage.
+ //pour que si le biniou ne bouge plus, il n'essaye plus de freiner tant
+ //qu'il n'y a pa un nouveau calcul de la vitesse.
+
+ tachySectionDist_cm += TACHY_CM;
+ tachyTotalDist_cm += TACHY_CM;
+ tachySpeed_cmps = (TACHY_CM * 1000000.0)/(double)timerSinceGetTachy.read_us(); // a chaque IT on a parcouru 8 cm soit (8*1000000)/durée
+#if DEBUG > 2
+ pc.printf("IT: distance parcourue %.4lf , vitesse:%.4lf \r\n",tachyTotalDist_cm,tachySpeed_cmps);
+#endif
+ timerSinceGetTachy.reset();
+ timerSinceGetTachy.start();
+ return;
+
#if DEBUG > 0
pc.printf("IT tachy\r\n");
#endif
@@ -305,8 +308,9 @@
//########## INIT STATES MACHINES ##########
void mursInit(void)
{
-#if DEBUG > 0
- pc.baud(115200);
+
+ pc.baud(115200); // pourquoi c'est la ?
+ #if DEBUG > 0
pc.printf("Init Murs\r\n");
#endif
timeSinceStart.start();
@@ -334,9 +338,9 @@
p_sectionCourante=&p_section1;
it_tachymeter.fall(&it4cm);
timerSinceGetTachy.start();
- getTachySpeed();//to reset
+ //getTachySpeed();//to reset
tachySectionDist_cm = 0;
- tachyStepsRegister = 0;
+ //tachyStepsRegister = 0;
//section de test 1
p_section1.nextSection =NULL;// &p_section2;
@@ -346,7 +350,7 @@
p_section1.coef_p_speed = 1;
p_section1.lidarWarningDist_cm = 120.0;
p_section1.lng_section_cm = 30000.0;//30m
- p_section1.coef_p = 0.02;
+ p_section1.coef_p = 4;
p_section1.coef_i = 0.00001;
p_section1.coef_d = 0.00001;
@@ -358,7 +362,7 @@
p_section2.coef_p_speed = 1.0;
p_section2.lidarWarningDist_cm = 300.0;
p_section2.lng_section_cm = 200.0;//2m
- p_section2.coef_p = 0.02;
+ p_section2.coef_p = 1;
p_section2.coef_i = 0.00001;
p_section2.coef_d = 0.00001;
@@ -383,7 +387,7 @@
pc.printf("Init Throttle\r\n");
#endif
st_thro = REGULATION_SPEED;
- PwmMotor.period_us(DIERCTION_PERIOD_MS); //20 ms is default
+ PwmMotor.period_us(DIRECTION_PERIOD_MS); //20 ms is default
PwmMotor.pulsewidth_us(1000);//MIN
wait(3);
PwmMotor.pulsewidth_us(2000);//MAX
@@ -392,8 +396,8 @@
wait(1);
pulseSpeed_us = INITAL_PULSE_SPEED_US;
#if DEBUG > 0
- pc.printf("temps init: %d micros\r\n",timeSinceStart.read_us());
- pc.printf("\r\nStates INIT: state Murs: %d, state Section %d, state MaxSpeed %d, state Throttle %d\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro);
+ pc.printf("temps init: %.4lf micros\r\n",timeSinceStart.read_us());
+ pc.printf("\r\nStates INIT: state Murs: %.4lf, state Section %.4lf, state MaxSpeed %.4lf, state Throttle %.4lf\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro);
timeSinceStart.reset();
timeSinceStart.start();
#endif
@@ -416,11 +420,16 @@
distMurG45Moy = getDistMoy(&anaG45,distMurG45,NB_ECHANTILLONS_IR);
distMurD45Moy = getDistMoy(&anaD45,distMurD45,NB_ECHANTILLONS_IR);
distMurG90Moy = getDistMoy(&anaG90,distMurG90,NB_ECHANTILLONS_IR);
- distMurD90Moy = getDistMoy(&anaG90,distMurD90,NB_ECHANTILLONS_IR);
- shortDistMurG90Moy = getShortDistMoy(&anaShortG90,shortDistMurD90,NB_ECHANTILLONS_IR);
- shortDistMurD90Moy = getShortDistMoy(&anaShortD90,shortDistMurD90,NB_ECHANTILLONS_IR);
+ distMurD90Moy = getDistMoy(&anaD90,distMurD90,NB_ECHANTILLONS_IR);
+ //shortDistMurG90Moy = getShortDistMoy(&anaShortG90,shortDistMurD90,NB_ECHANTILLONS_IR);
+ //shortDistMurD90Moy = getShortDistMoy(&anaShortD90,shortDistMurD90,NB_ECHANTILLONS_IR);
index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR;
+
+// pour debug je supprime la fonction avec les courts et les longs
+trueDistMurD90Moy = distMurD90Moy;
+trueDistMurG90Moy = distMurG90Moy;
+/*
if(shortDistMurG90Moy < DIST_MIN_LONG_CM)
{
trueDistMurG90Moy = shortDistMurG90Moy;
@@ -434,7 +443,8 @@
}else
{
trueDistMurD90Moy = distMurD90Moy;
- }
+ }*/
+
#ifdef DLVV
switch (st_obstacle) {
@@ -531,7 +541,7 @@
#if (DEBUG > 3)
pc.printf("\r\nUpdate Throttle\r\n");
#endif
- getTachySpeed();
+ //getTachySpeed();
switch (st_thro) {
case REGULATION_SPEED:
if( st_currentSection == ARRET ||
@@ -598,19 +608,23 @@
switch (st_murs) {
case REF_A_GAUCHE://par defaut, on compte à partir de la bordure gauche
- largeurPiste = 150.0;
+ largeurPiste90 = 150.0;
positionSurPiste90 = ( trueDistMurG90Moy + DEMI_LARGEUR_BINIOU_CM );
positionSurPiste45 = ( distMurG45Moy * 1.414214/2.0 ) + DEMI_LARGEUR_BINIOU_CM;
+ pc.printf("REF_A_GAUCHE\r\n");
break;
case REF_A_DROITE://par defaut, on compte à partir de la bordure gauche
- largeurPiste = 150.0;
- positionSurPiste90 = largeurPiste - ( DEMI_LARGEUR_BINIOU_CM + trueDistMurD90Moy ) ;
- positionSurPiste45 = largeurPiste - ( distMurD45Moy * (1.414214)/2 ) + DEMI_LARGEUR_BINIOU_CM;
+ largeurPiste90 = 150.0;
+ positionSurPiste90 = largeurPiste90 - ( DEMI_LARGEUR_BINIOU_CM + trueDistMurD90Moy ) ;
+ positionSurPiste45 = largeurPiste90 - ( distMurD45Moy * (1.414214)/2 ) + DEMI_LARGEUR_BINIOU_CM;
+ pc.printf("REF_A_DROITE\r\n");
break;
default://REF_BIDIR
- largeurPiste = trueDistMurG90Moy + trueDistMurD90Moy + (2 * DEMI_LARGEUR_BINIOU_CM);
- positionSurPiste90 = (trueDistMurG90Moy + DEMI_LARGEUR_BINIOU_CM);
- positionSurPiste45 = (largeurPiste / 2) + ((distMurG45Moy - distMurD45Moy) * (1.414214)/2 );
+ largeurPiste90 = trueDistMurG90Moy + trueDistMurD90Moy ;
+ largeurPiste45 = distMurG45Moy + distMurD45Moy ;
+ positionSurPiste90 = (trueDistMurG90Moy );
+ positionSurPiste45 = (distMurG45Moy);
+ pc.printf("default\r\n");
break;
}
@@ -618,6 +632,20 @@
//deriv correction
derive45 = positionSurPiste45 - positionSurPiste45Prev;
derive90 = positionSurPiste90 - positionSurPiste90Prev;
+
+ //pc.printf("derive45 => %.4lf \r\n ",derive45);
+ // pc.printf("derive90 => %.4lf \r\n ",derive90);
+ /*
+ pc.printf("distMurG45Moy => %.4lf \r\n ",distMurG45Moy);
+ pc.printf("distMurD45Moy => %.4lf \r\n ",distMurD45Moy);
+ pc.printf("trueDistMurG90Moy => %.4lf \r\n ",trueDistMurG90Moy);
+ pc.printf("trueDistMurD90Moy => %.4lf \r\n ",trueDistMurD90Moy);
+ pc.printf("positionSurPiste90 => %.4lf \r\n ",positionSurPiste90);
+ pc.printf("positionSurPiste45 => %.4lf \r\n ",positionSurPiste45);
+ pc.printf("largeurPiste90 => %.4lf \r\n ",largeurPiste90);
+ */
+
+
//integral correction
lastDifferences90[lastDifferenceIndex] = (p_sectionCourante->consigne_position - positionSurPiste90);
integralSum=0;
@@ -625,30 +653,39 @@
integralSum+=lastDifferences90[f];
}
lastDifferenceIndex = (lastDifferenceIndex + 1)%NB_INTEGRAL_SAMPLES;
-
+
+
+
//application des coefficients
- pulseDirection_us = (int)(COEF_RAYON_BR_S_DIST/tachySpeed_cmps * //facteur de vitesse: plus on va vite, moins on corrige
+ pulseDirection_us_temp = ((positionSurPiste45 - (largeurPiste45/2.0)) * 4)
+ + DIRECTION_PULSE_MIDDLE;
+ /*pulseDirection_us_temp = COEF_RAYON_BR_S_DIST/tachySpeed_cmps * //facteur de vitesse: plus on va vite, moins on corrige
(
( (p_sectionCourante->consigne_position - positionSurPiste45) * p_sectionCourante->coef_p)
- + ( derive45 * p_sectionCourante->coef_d)
- + ( integralSum * p_sectionCourante->coef_i)
- ))
- + DIRECTION_PULSE_MIDDLE;
-
+ //+ ( derive45 * p_sectionCourante->coef_d)
+ //+ ( integralSum * p_sectionCourante->coef_i)
+ )
+ + DIRECTION_PULSE_MIDDLE;*/
+ //pc.printf("\r %.4lf ",pulseDirection_us);
+ pulseDirection_us = (int)pulseDirection_us_temp ;
+ pc.printf("p_sectionCourante->consigne_position => %.4lf \r\n ",p_sectionCourante->consigne_position);
+ pc.printf("positionSurPiste45 => %.4lf \r\n ",positionSurPiste45);
+ pc.printf("p_sectionCourante->coef_p => %.4lf \r\n ",p_sectionCourante->coef_p);
+ pc.printf("pulseDirection_us_temp => %.4lf \r\n ",pulseDirection_us_temp);
//gestioon du dépassement
if(pulseDirection_us > DIRECTION_PULSE_MAX) { //POUR TOURNER A GAUCHE
#if DEBUG > 1
- pc.printf("!!! OVER PWM Direction pulse: %d\r\n",pulseDirection_us);
+ pc.printf("!!! OVER PWM Direction pulse: %.4lf\r\n",pulseDirection_us);
#endif
pulseDirection_us = DIRECTION_PULSE_MAX;
} else if(pulseDirection_us < DIRECTION_PULSE_MIN ) { //POUR TOURNER A DROITE
#if DEBUG > 1
- pc.printf("!!! UNDER PWM Direction pulse: %d\r\n",pulseDirection_us);
+ pc.printf("!!! UNDER PWM Direction pulse: %.4lf\r\n",pulseDirection_us);
#endif
pulseDirection_us = DIRECTION_PULSE_MIN ;
}
#if DEBUG > 1
- pc.printf("PWM Direction pulse: %d\r\n",pulseDirection_us);
+ pc.printf("PWM Direction pulse: %.4lf\r\n",pulseDirection_us);
#endif
PwmDirection.pulsewidth_us(pulseDirection_us);
return;
@@ -728,7 +765,7 @@
PwmMotor.pulsewidth_us(pulseSpeed_us);
#if DEBUG > 1
- pc.printf("PWM Thro pulse: %d micros\r\n",pulseSpeed_us);
+ pc.printf("PWM Thro pulse: %.4lf micros\r\n",pulseSpeed_us);
#endif
return;
}