![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TRR2018 omar
Fork of biniou by
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; }