Hlimi Omar
TRR2018 omar
Diff: stateMachines.cpp
- Revision:
- 43:d2164ee3336a
- Parent:
- 42:3f12252862b9
- Child:
- 44:69751c05a8f3
--- 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;
}