![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TRR2018 omar
Fork of biniou by
Diff: stateMachines.cpp
- Revision:
- 41:4dd36f607279
- Parent:
- 40:b9450d753782
- Child:
- 42:3f12252862b9
diff -r b9450d753782 -r 4dd36f607279 stateMachines.cpp --- a/stateMachines.cpp Wed Sep 19 13:13:37 2018 +0000 +++ b/stateMachines.cpp Wed Sep 19 13:42:04 2018 +0000 @@ -170,7 +170,7 @@ history[indexSample].pwm_thro_us = pulseSpeed_us; history[indexSample].pwm_dir_us = pulseDirection_us; history[indexSample].dist = tachySectionDist_cm, - history[indexSample].distLidar = distLidar; + history[indexSample].distLidar = distLidar; history[indexSample].strLidar = strengthLidar; indexSample++; #if DEBUG > 0 @@ -226,8 +226,7 @@ void initIntegrationTable() { - for(int h=0;h<NB_INTEGRAL_SAMPLES;h++) - { + for(int h=0; h<NB_INTEGRAL_SAMPLES; h++) { lastDifferences90[h] = 0; } return; @@ -238,6 +237,9 @@ { tab[index_fifo_ir] = 3.3 * (double)p->read(); // on convertit directement en volts //tension proportionelle à l'inverse de la distance en dessous de 2V + if(tab[index_fifo_ir]< 0.1034) { + tab[index_fifo_ir]= 0.11; + } tab[index_fifo_ir] = (11.3531/(tab[index_fifo_ir]-0.1034))-0.42; int sumMoy = 0; @@ -260,11 +262,10 @@ } void tachyCheck() { - if(timerSinceTachy.read_us() > 500000) - { + if(timerSinceTachy.read_us() > 500000) { tachySpeed_cmps = 0.0; } -return; + return; } @@ -429,43 +430,40 @@ index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR; -trueDistMurD90Moy = distMurD90Moy; -trueDistMurG90Moy = distMurG90Moy; + trueDistMurD90Moy = distMurD90Moy; + trueDistMurG90Moy = distMurG90Moy; - if(shortDistMurG90Moy < DIST_MIN_LONG_CM) - { + if(shortDistMurG90Moy < DIST_MIN_LONG_CM) { trueDistMurG90Moy = shortDistMurG90Moy; - }else - { + } else { trueDistMurG90Moy = distMurG90Moy; } - if(shortDistMurD90Moy < DIST_MIN_LONG_CM) - { + if(shortDistMurD90Moy < DIST_MIN_LONG_CM) { trueDistMurD90Moy = shortDistMurD90Moy; - }else - { + } else { trueDistMurD90Moy = distMurD90Moy; } #ifdef DLVV switch (st_obstacle) { - case FRONT_OBSTRUCTED: - st_murs = REF_A_GAUCHE; - return; - case RIGHT_OBSTRUCTED: - st_murs = REF_A_GAUCHE; - return; - case LEFT_OBSTRUCTED: - st_murs = REF_A_DROITE; - return; - default: - break; + case FRONT_OBSTRUCTED: + st_tmpMurs = REF_A_GAUCHE; + return; + case RIGHT_OBSTRUCTED: + st_tmpMurs = REF_A_GAUCHE; + return; + case LEFT_OBSTRUCTED: + st_tmpMurs = REF_A_DROITE; + return; + default: + break; } #endif - st_murs = REF_BIDIR; + st_tmpMurs = REF_BIDIR; + st_murs = st_tmpMurs; return; } @@ -515,19 +513,18 @@ #if (DEBUG > 3) pc.printf("\r\nUpdate MaxSpeed\r\n"); #endif - i=0; - if( strengthLidar > LIDAR_STRENGTH_THRESOLD ) { + if(st_maxSpeed == BLOCKED && strengthLidar > LIDAR_STRENGTH_THRESOLD && distLidar < p_sectionCourante->lidarWarningDist_cm) { + st_tmpMaxSpeed = BLOCKED; + } else if( strengthLidar > LIDAR_STRENGTH_THRESOLD ) { if( distLidar > p_sectionCourante->lidarWarningDist_cm ) { st_tmpMaxSpeed = SPEED_MAX; } else if( strengthLidar > LIDAR_STRENGTH_THRESOLD && strengthLidarPrev > LIDAR_STRENGTH_THRESOLD && distLidar < p_sectionCourante->lidarWarningDist_cm && - distLidarPrev-distLidar > 3) - { + distLidarPrev-distLidar > 3) { st_tmpMaxSpeed = BLOCKED; - } - else{ + } else { st_tmpMaxSpeed = SPEED_WARNING; } } else { @@ -546,39 +543,34 @@ switch (st_thro) { case REGULATION_SPEED: if( st_currentSection == ARRET || - st_maxSpeed == BLOCKED ) - { + st_maxSpeed == BLOCKED ) { st_tmpThro = BRAKING; } #ifdef FREINAGE_ADAPTATIF - else if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS)) - { + else if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS)) { st_tmpThro = BRAKING; brakingDurationNeeded_us = 10000.0 * MASSE_BINIOU_KG *((tachySpeed_cmps-maxSpeed_cmps)*(tachySpeed_cmps+maxSpeed_cmps)) / (PUISSANCE_FREINAGE_W *2) ;//vitesse en m/s et temps en us brakingTimer.reset(); brakingTimer.start(); } #endif - else - { + else { return; } break; case BRAKING: if( st_maxSpeed == BLOCKED - #ifdef FREINAGE_ADAPTATIF - || brakingTimer.read_us() < brakingDurationNeeded_us - #endif - ) - { +#ifdef FREINAGE_ADAPTATIF + || brakingTimer.read_us() < brakingDurationNeeded_us +#endif + ) { st_tmpThro = BRAKING; - }else - if(st_currentSection == ARRET) { + } else if(st_currentSection == ARRET) { st_tmpThro = STOPPED; - } else{ - #ifdef FREINAGE_ADAPTATIF + } else { +#ifdef FREINAGE_ADAPTATIF brakingDurationNeeded_us = 0; - #endif +#endif st_tmpThro = REGULATION_SPEED; } break; @@ -608,31 +600,31 @@ #endif switch (st_murs) { - case REF_A_GAUCHE://par defaut, on compte à partir de la bordure gauche - largeurPiste90 = 150.0; - positionSurPiste90 = ( trueDistMurG90Moy + DEMI_LARGEUR_BINIOU_CM ); - positionSurPiste45 = ( distMurG45Moy * 1.414214/2.0 ) + DEMI_LARGEUR_BINIOU_CM; + case REF_A_GAUCHE://par defaut, on compte à partir de la bordure gauche + largeurPiste90 = 150.0; + positionSurPiste90 = ( trueDistMurG90Moy + DEMI_LARGEUR_BINIOU_CM ); + positionSurPiste45 = ( distMurG45Moy * 1.414214/2.0 ) + DEMI_LARGEUR_BINIOU_CM; #ifdef OMAR - pc.printf("REF_A_GAUCHE\r\n"); + pc.printf("REF_A_GAUCHE\r\n"); #endif - break; - case REF_A_DROITE://par defaut, on compte à partir de la bordure gauche - largeurPiste90 = 150.0; - positionSurPiste90 = largeurPiste90 - ( DEMI_LARGEUR_BINIOU_CM + trueDistMurD90Moy ) ; - positionSurPiste45 = largeurPiste90 - ( distMurD45Moy * (1.414214)/2 ) + DEMI_LARGEUR_BINIOU_CM; + break; + case REF_A_DROITE://par defaut, on compte à partir de la bordure gauche + largeurPiste90 = 150.0; + positionSurPiste90 = largeurPiste90 - ( DEMI_LARGEUR_BINIOU_CM + trueDistMurD90Moy ) ; + positionSurPiste45 = largeurPiste90 - ( distMurD45Moy * (1.414214)/2 ) + DEMI_LARGEUR_BINIOU_CM; #ifdef OMAR - pc.printf("REF_A_DROITE\r\n"); + pc.printf("REF_A_DROITE\r\n"); #endif - break; - default://REF_BIDIR - largeurPiste90 = trueDistMurG90Moy + trueDistMurD90Moy ; - largeurPiste45 = distMurG45Moy + distMurD45Moy ; - positionSurPiste90 = (trueDistMurG90Moy ); - positionSurPiste45 = (distMurG45Moy); + break; + default://REF_BIDIR + largeurPiste90 = trueDistMurG90Moy + trueDistMurD90Moy ; + largeurPiste45 = distMurG45Moy + distMurD45Moy ; + positionSurPiste90 = (trueDistMurG90Moy ); + positionSurPiste45 = (distMurG45Moy); #ifdef OMAR - pc.printf("REF_BIDIR\r\n"); + pc.printf("REF_BIDIR\r\n"); #endif - break; + break; } @@ -652,7 +644,7 @@ pc.printf("largeurPiste90 => %.4lf \r\n ",largeurPiste90);*/ #endif - //integral correction + //integral correction lastDifferences90[lastDifferenceIndex] = (p_sectionCourante->consigne_position - positionSurPiste90); integralSum=0; for(int f=0; f<NB_INTEGRAL_SAMPLES; f++) { @@ -660,27 +652,25 @@ } lastDifferenceIndex = (lastDifferenceIndex + 1)%NB_INTEGRAL_SAMPLES; - - - + + + //application des coefficients - pulseDirection_us_temp = ( - ((positionSurPiste45 - (largeurPiste45/2.0)) * p_sectionCourante->coef_p) - + ((positionSurPiste90 - (largeurPiste90/2.0)) * p_sectionCourante->coef_p) - + ( -derive45 * p_sectionCourante->coef_d) - + ( -derive90 * p_sectionCourante->coef_d) - + ( integralSum * p_sectionCourante->coef_i) - )* (300.0/(tachySpeed_cmps+1.0)) // faire du test reel pour afiné ou un calcul - + DIRECTION_PULSE_MIDDLE; - - pulseDirection_us = (int)pulseDirection_us_temp ; - - + + pulseDirection_us = (int)( + ((positionSurPiste45 - (largeurPiste45/2.0)) * p_sectionCourante->coef_p) + + ((positionSurPiste90 - (largeurPiste90/2.0)) * p_sectionCourante->coef_p) + + ( -derive45 * p_sectionCourante->coef_d) + + ( -derive90 * p_sectionCourante->coef_d) + + ( integralSum * p_sectionCourante->coef_i) + )* (300.0/(tachySpeed_cmps+1.0)) // faire du test reel pour afiné ou un calcul + + DIRECTION_PULSE_MIDDLE; + + #ifdef OMAR - 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); + 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); #endif //gestioon du dépassement if(pulseDirection_us > DIRECTION_PULSE_MAX) { //POUR TOURNER A GAUCHE @@ -736,7 +726,7 @@ #endif switch(st_maxSpeed) { case BLOCKED: - maxSpeed_cmps = 0; + maxSpeed_cmps = 0; break; case SPEED_WARNING: maxSpeed_cmps = p_sectionCourante->slowSpeed_cmps;