Hlimi Omar
/
biniou
TRR2018 omar
Fork of biniou by
Diff: stateMachines.cpp
- Revision:
- 43:d2164ee3336a
- Parent:
- 42:3f12252862b9
- Child:
- 44:69751c05a8f3
diff -r 3f12252862b9 -r d2164ee3336a stateMachines.cpp --- a/stateMachines.cpp Wed Sep 19 20:44:07 2018 +0000 +++ b/stateMachines.cpp Thu Sep 20 11:42:06 2018 +0000 @@ -482,30 +482,36 @@ #if (DEBUG > 3) pc.printf("\r\nUpdate Section\r\n"); #endif - switch (st_currentSection) { - case RUNNING_SECTION: - if(tachySectionDist_cm > p_sectionCourante->lng_section_cm) { //on pourrait rajouter un test lidar - st_tmpSection = LOADING_SECTION; - } else { - return; - } - break; - case LOADING_SECTION: - if(p_sectionCourante != NULL) { //la section a ete chargee dans sectionOutput - st_tmpSection = RUNNING_SECTION; - } else { - st_tmpSection=ARRET; - } - break; - case ARRET: - if(p_sectionCourante != NULL) { - st_tmpSection = RUNNING_SECTION; - } else { - return; - } - break; - default: - break; + + if(p_sectionCourante == NULL) { + st_tmpSection = ARRET; + } else { + + switch (st_currentSection) { + case RUNNING_SECTION: + if(tachySectionDist_cm > p_sectionCourante->lng_section_cm) { //on pourrait rajouter un test lidar + st_tmpSection = LOADING_SECTION; + } else { + return; + } + break; + case LOADING_SECTION: + if(p_sectionCourante != NULL) { //la section a ete chargee dans sectionOutput + st_tmpSection = RUNNING_SECTION; + } else { + st_tmpSection=ARRET; + } + break; + case ARRET: + if(p_sectionCourante != NULL) { + st_tmpSection = RUNNING_SECTION; + } else { + return; + } + break; + default: + break; + } } st_currentSection = st_tmpSection; return; @@ -516,8 +522,9 @@ #if (DEBUG > 3) pc.printf("\r\nUpdate MaxSpeed\r\n"); #endif - - if(st_maxSpeed == BLOCKED && strengthLidar > LIDAR_STRENGTH_THRESOLD && distLidar < p_sectionCourante->lidarWarningDist_cm) { + if(p_sectionCourante != NULL) { + st_tmpMaxSpeed = BLOCKED; + } else 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 ) { @@ -598,103 +605,105 @@ //updating output parameters void mursOutput(void) { - //pour dériver - positionSurPiste45Prev = positionSurPiste45; - positionSurPiste90Prev = positionSurPiste90; + if(p_sectionCourante != NULL) { + //pour dériver + positionSurPiste45Prev = positionSurPiste45; + positionSurPiste90Prev = positionSurPiste90; #if (DEBUG > 3) - pc.printf("\r\n Output MURS\r\n"); + pc.printf("\r\n Output MURS\r\n"); #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; + 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; #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; + } - //deriv correction - derive45 = positionSurPiste45 - positionSurPiste45Prev; - derive90 = positionSurPiste90 - positionSurPiste90Prev; + //deriv correction + derive45 = positionSurPiste45 - positionSurPiste45Prev; + derive90 = positionSurPiste90 - positionSurPiste90Prev; #ifdef OMAR/* - 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);*/ + 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);*/ #endif - //integral correction - lastDifferences90[lastDifferenceIndex] = (p_sectionCourante->consigne_position - positionSurPiste90); - integralSum=0; - for(int f=0; f<NB_INTEGRAL_SAMPLES; f++) { - integralSum+=lastDifferences90[f]; - } - lastDifferenceIndex = (lastDifferenceIndex + 1)%NB_INTEGRAL_SAMPLES; + //integral correction + lastDifferences90[lastDifferenceIndex] = (p_sectionCourante->consigne_position - positionSurPiste90); + integralSum=0; + for(int f=0; f<NB_INTEGRAL_SAMPLES; f++) { + integralSum+=lastDifferences90[f]; + } + lastDifferenceIndex = (lastDifferenceIndex + 1)%NB_INTEGRAL_SAMPLES; - //application des coefficients + //application des coefficients - 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; + 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("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 + if(pulseDirection_us > DIRECTION_PULSE_MAX) { //POUR TOURNER A GAUCHE #if DEBUG > 1 - pc.printf("!!! OVER PWM Direction pulse: %.4lf\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 + 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: %.4lf\r\n",pulseDirection_us); + pc.printf("!!! UNDER PWM Direction pulse: %.4lf\r\n",pulseDirection_us); #endif - pulseDirection_us = DIRECTION_PULSE_MIN ; - } + pulseDirection_us = DIRECTION_PULSE_MIN ; + } #if DEBUG > 1 - pc.printf("PWM Direction pulse: %.4lf\r\n",pulseDirection_us); + pc.printf("PWM Direction pulse: %.4lf\r\n",pulseDirection_us); #endif - PwmDirection.pulsewidth_us(pulseDirection_us); - return; + PwmDirection.pulsewidth_us(pulseDirection_us); + return; + } } #ifdef DLVV @@ -708,18 +717,20 @@ #if (DEBUG > 3) pc.printf("\r\n Output Section\r\n"); #endif - switch (st_currentSection) { - case RUNNING_SECTION: - break; - case LOADING_SECTION: - p_sectionCourante=p_sectionCourante->nextSection; - tachySectionDist_cm = 0; - break; - case ARRET: - //on est à l'arret - break; - default: - break; + if(p_sectionCourante !=NULL) { + switch (st_currentSection) { + case RUNNING_SECTION: + break; + case LOADING_SECTION: + p_sectionCourante=p_sectionCourante->nextSection; + tachySectionDist_cm = 0; + break; + case ARRET: + //on est à l'arret + break; + default: + break; + } } return; } @@ -730,16 +741,20 @@ #if (DEBUG > 3) pc.printf("\r\n Output MAX SPEED\r\n"); #endif - switch(st_maxSpeed) { - case BLOCKED: - maxSpeed_cmps = 0; - break; - case SPEED_WARNING: - maxSpeed_cmps = p_sectionCourante->slowSpeed_cmps; - break; - default: - maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps; - break; + if(p_sectionCourante !=NULL) { + switch(st_maxSpeed) { + case BLOCKED: + maxSpeed_cmps = 0; + break; + case SPEED_WARNING: + maxSpeed_cmps = p_sectionCourante->slowSpeed_cmps; + break; + default: + maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps; + break; + } + } else { + maxSpeed_cmps = 0.0; } return; }