Hlimi Omar
TRR2018 omar
Diff: stateMachines.cpp
- Revision:
- 41:4dd36f607279
- Parent:
- 40:b9450d753782
- Child:
- 42:3f12252862b9
--- 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;