Hlimi Omar
/
biniou
TRR2018 omar
Fork of biniou by
Diff: stateMachines.cpp
- Revision:
- 29:fc984fe08ca7
- Parent:
- 27:f8c3f1524a64
- Child:
- 30:21642fb8297a
diff -r af53caf04446 -r fc984fe08ca7 stateMachines.cpp --- a/stateMachines.cpp Mon Sep 17 11:08:02 2018 +0000 +++ b/stateMachines.cpp Tue Sep 18 18:28:19 2018 +0000 @@ -2,31 +2,36 @@ #if DEBUG >0 Serial pc(USBTX, USBRX); // tx, rx #endif -#if DEBUG >= -1 -InterruptIn my_button(USER_BUTTON); + +//*************** declarations *************** #ifdef DUMP_SAMPLIG_PERIOD Timer timerLog; #endif -#endif -uint16_t distMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne -uint16_t distMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit pour moyenne -uint16_t distMurG45[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 45deg pour moyenne -uint16_t distMurD45[NB_ECHANTILLONS_IR];//buffer tournant ir avant droit 45deg pour moyenne +double distMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne +double distMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit pour moyenne +double shortDistMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche petites distances pour moyenne +double shortDistMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit petites distances pour moyenne +double distMurG45[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 45deg pour moyenne +double distMurD45[NB_ECHANTILLONS_IR];//buffer tournant ir avant droit 45deg pour moyenne #ifdef DLVV uint16_t distMurG10[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 10deg pour moyenne uint16_t distMurD10[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit 10deg pour moyenne uint16_t distMurFront[NB_ECHANTILLONS_IR];//buffer tournant ir front #endif -uint16_t distMurG90Moy; -uint16_t distMurD90Moy; -uint16_t distMurG45Moy; -uint16_t distMurD45Moy; +double distMurG90Moy; +double distMurD90Moy; +double shortDistMurG90Moy; +double shortDistMurD90Moy; +double trueDistMurG90Moy; +double trueDistMurD90Moy; +double distMurG45Moy; +double distMurD45Moy; #ifdef DLVV -uint16_t distMurG10Moy; -uint16_t distMurD10Moy; -uint16_t distMurFrontMoy +double distMurG10Moy; +double distMurD10Moy; +double distMurFrontMoy #endif int index_fifo_ir = 0;//pour géreer le buffer tournant int index_fifo_lidar = 0; @@ -41,11 +46,10 @@ int pulseDirection_us = DIRECTION_PULSE_MIDDLE; int pulseSpeed_us = INITAL_PULSE_SPEED_US; //Capteurs direction - - - AnalogIn anaG90(CAPT_90_GAUCHE);//capteur ir coté gauche AnalogIn anaD90(CAPT_90_DROITE);//capteur ir coté droit +AnalogIn anaShortG90(CAPT_90_GAUCHE_SHORT);//capteur ir coté gauche short +AnalogIn anaShortD90(CAPT_90_DROITE_SHORT);//capteur ir coté droit short AnalogIn anaG45(CAPT_45_GAUCHE);//capteur ir avant gauche 45 deg AnalogIn anaD45(CAPT_45_DROITE);//capteur ir avant droit 45deg #ifdef DLVV @@ -54,7 +58,14 @@ AnalogIn anaDlvvFront(CAPT_DEVANT);//capteur ir avant #endif -int differenceGD45,differenceGD90,prevDiffGD90,prevDiffGD45,derive45,derive90; +//piste +double largeurPiste = 150.0; +double positionSurPiste90 = 75.0; +double positionSurPiste90Prev = positionSurPiste90; +double positionSurPiste45 = 75.0; +double positionSurPiste45Prev = positionSurPiste45; + +int derive45,derive90; int lastDifferences90[NB_INTEGRAL_SAMPLES] = {0};//for integral correction int lastDifferenceIndex = 0; int integralSum; @@ -76,10 +87,10 @@ const int HEADER=0x59;// data package frame header //SPEED -int maxSpeed_cmps = 0; -int tachySpeed_cmps = 0; -int tachyStepsRegister = 0; -int tachySectionDist_cm = 0; +double maxSpeed_cmps = 0; +double tachySpeed_cmps = 0; +double tachyStepsRegister = 0; +double tachySectionDist_cm = 0; #ifdef FREINAGE_ADAPTATIF Timer brakingTimer; @@ -119,8 +130,10 @@ history[m].states.maxSpeed = '0'; history[m].states.throttle = '0'; history[m].time = 0; - history[m].diffgd45 = 0; - history[m].diffgd90 = 0; + history[m].position45 = 0.0; + history[m].position90 = 0.0; + history[m].largeurPiste = 0.0; + history[m].dist = 0.0; history[m].pwm_thro_us = 0; history[m].pwm_dir_us = 0; history[m].distLidar = 0; @@ -148,12 +161,13 @@ history[indexSample].states.maxSpeed = (char)st_maxSpeed; history[indexSample].states.throttle = (char)st_thro; history[indexSample].time = timeSinceStart.read_us() ; - history[indexSample].diffgd45 = differenceGD45; - history[indexSample].diffgd90 = differenceGD90; + history[indexSample].position45 = positionSurPiste45; + history[indexSample].position90 = positionSurPiste90; + history[indexSample].largeurPiste = largeurPiste; 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 @@ -174,12 +188,12 @@ #if DEBUG > 0 pc.printf("[START TO TRANSMIT]\r\n"); #endif - serialLidar.printf("time,diffgd 45,diffgd 90,pwm_thro_us,pwm_dir_us,dist,murs/dlvv,section,maxSpeed,throttle\r\n"); + serialLidar.printf("time,position 45,position 90,largeur pistepwm_thro_us,pwm_dir_us,dist,murs/dlvv,section,maxSpeed,throttle\r\n"); for(int p=0; p<indexSample; p++) { - serialLidar.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n", + serialLidar.printf("%d,%.5f,%.5f,%.5f,%d,%d,%.5f,%d,%d,%d,%d,%d\r\n", history[p].time, - history[p].diffgd45, - history[p].diffgd90, + history[p].position45, + history[p].position90, history[p].pwm_thro_us, history[p].pwm_dir_us, history[p].dist, @@ -206,6 +220,15 @@ } #endif +void initIntegrationTable() +{ + for(int h=0;h<NB_INTEGRAL_SAMPLES;h++) + { + lastDifferences90[h] = 0; + } + return; +} + void getTachySpeed() { //tachySteps = VALEUR_DU_PIN; @@ -218,15 +241,31 @@ #if DEBUG > 2 pc.printf("IT: distance parcourue %d , vitesse:%d \r\n",tachySectionDist_cm,tachySpeed_cmps); #endif - tachyStepsRegister=0; + tachyStepsRegister=0.0; timerSinceGetTachy.reset(); timerSinceGetTachy.start(); return; } -uint16_t getDistMoy(uint16_t* tab,int size) +double getShortDistMoy(AnalogIn* p,double* tab,int size) { + 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 + tab[index_fifo_ir] = 20.0/(tab[index_fifo_ir]-0.3); + + int sumMoy = 0; + for(int k=0; k<size; k++) { + sumMoy+=tab[k]; + } + return sumMoy/size; +} + +double getDistMoy(AnalogIn* p,double* tab,int size) +{ + 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]); int sumMoy = 0; for(int k=0; k<size; k++) { sumMoy+=tab[k]; @@ -236,7 +275,7 @@ void it4cm() { - tachyStepsRegister+=TACHY_CM; + tachyStepsRegister += TACHY_CM; #if DEBUG > 0 pc.printf("IT tachy\r\n"); #endif @@ -266,20 +305,14 @@ //########## INIT STATES MACHINES ########## void mursInit(void) { -#if DEBUG >= -1 - my_button.fall(&pressed); - initSamples(); -#endif #if DEBUG > 0 pc.baud(115200); pc.printf("Init Murs\r\n"); #endif timeSinceStart.start(); - st_murs=EQUILIBRAGE_REPULSIF; + st_murs=REF_BIDIR; PwmDirection.period_us(SPEED_PERIOD_US); PwmDirection.pulsewidth_us(DIRECTION_PULSE_MIDDLE);// milieu - prevDiffGD45 = 0; - prevDiffGD90 = 0; return; } #ifdef DLVV @@ -288,7 +321,7 @@ #if DEBUG > 0 pc.printf("Init Obstacle\r\n"); #endif - st_speedLimit=FRONT_CLEAR; + st_speedLimit=ALL_CLEAR; return; } #endif @@ -307,27 +340,27 @@ //section de test 1 p_section1.nextSection =NULL;// &p_section2; - p_section1.targetSpeed_cmps = 400; - p_section1.slowSpeed_cmps = 328; - p_section1.brakingCoefficient = 61; // application de la formule + p_section1.consigne_position = 75.0; + p_section1.targetSpeed_cmps = 400.0; + p_section1.slowSpeed_cmps = 328.0; p_section1.coef_p_speed = 1; - p_section1.lidarWarningDist_cm = 120; - p_section1.lng_section_cm = 30000;//30m - p_section1.coef_p = 50;//35; - p_section1.coef_i = 10000;//35*NB_INTEGRAL_SAMPLES; - p_section1.coef_d = 10000;//35; + p_section1.lidarWarningDist_cm = 120.0; + p_section1.lng_section_cm = 30000.0;//30m + p_section1.coef_p = 0.02; + p_section1.coef_i = 0.00001; + p_section1.coef_d = 0.00001; //section de test p_section2.nextSection = NULL; - p_section2.targetSpeed_cmps = 328; - p_section2.slowSpeed_cmps = 328; - p_section2.brakingCoefficient = 0; // application de la formule - p_section2.coef_p_speed = 1; - p_section2.lidarWarningDist_cm = 300; - p_section2.lng_section_cm = 200;//2m - p_section2.coef_p = 50; - p_section2.coef_i = 10000;//35*NB_INTEGRAL_SAMPLES; - p_section2.coef_d = 10000;//35; + p_section2.consigne_position = 75.0; + p_section2.targetSpeed_cmps = 328.0; + p_section2.slowSpeed_cmps = 328.0; + 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_i = 0.00001; + p_section2.coef_d = 0.00001; return; } @@ -379,16 +412,48 @@ pc.printf("\r\nUpdate MURS\r\n"); #endif //lectures - distMurG90[index_fifo_ir]=anaG90.read_u16(); - distMurD90[index_fifo_ir]=anaD90.read_u16(); - distMurG45[index_fifo_ir]=anaG45.read_u16(); - distMurD45[index_fifo_ir]=anaD45.read_u16(); + + 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); index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR; - - distMurG45Moy = getDistMoy(distMurG45,NB_ECHANTILLONS_IR); - distMurD45Moy = getDistMoy(distMurD45,NB_ECHANTILLONS_IR); - distMurG90Moy = getDistMoy(distMurG90,NB_ECHANTILLONS_IR); - distMurD90Moy = getDistMoy(distMurD90,NB_ECHANTILLONS_IR); + + if(shortDistMurG90Moy < DIST_MIN_LONG_CM) + { + trueDistMurG90Moy = shortDistMurG90Moy; + }else + { + trueDistMurG90Moy = distMurG90Moy; + } + if(shortDistMurD90Moy < DIST_MIN_LONG_CM) + { + trueDistMurD90Moy = shortDistMurD90Moy; + }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; + } + +#endif + st_murs = REF_BIDIR; + return; } @@ -443,7 +508,7 @@ if( strengthLidar > LIDAR_STRENGTH_THRESOLD ) { if( distLidar > p_sectionCourante->lidarWarningDist_cm ) { st_tmpMaxSpeed = SPEED_MAX; - } else if( strengthLidar > LIDAR_STRENGTH_THRESOLD && + } else if( strengthLidar > LIDAR_STRENGTH_THRESOLD && strengthLidarPrev > LIDAR_STRENGTH_THRESOLD && distLidar < p_sectionCourante->lidarWarningDist_cm && distLidarPrev-distLidar > 3) @@ -470,15 +535,15 @@ 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)) { st_tmpThro = BRAKING; - brakingDurationNeeded_us = (10000)* MASSE_BINIOU_KG *((tachySpeed_cmps-maxSpeed_cmps)*(tachySpeed_cmps+maxSpeed_cmps)) / (PUISSANCE_FREINAGE_W *2) ;//vitesse en m/s et temps en us + 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(); } @@ -489,7 +554,7 @@ } break; case BRAKING: - if( BLOCKED + if( st_maxSpeed == BLOCKED #ifdef FREINAGE_ADAPTATIF || brakingTimer.read_us() < brakingDurationNeeded_us #endif @@ -524,21 +589,37 @@ //updating output parameters void mursOutput(void) { - prevDiffGD90 = differenceGD90; - prevDiffGD45 = differenceGD45; + //pour dériver + positionSurPiste45Prev = positionSurPiste45; + positionSurPiste90Prev = positionSurPiste90; #if (DEBUG > 3) pc.printf("\r\n Output MURS\r\n"); #endif - differenceGD90 = distMurD90Moy - distMurG90Moy; - differenceGD45 = distMurD45Moy - distMurG45Moy; + switch (st_murs) { + case REF_A_GAUCHE://par defaut, on compte à partir de la bordure gauche + largeurPiste = 150.0; + positionSurPiste90 = ( trueDistMurG90Moy + DEMI_LARGEUR_BINIOU_CM ); + positionSurPiste45 = ( distMurG45Moy * 1.414214/2.0 ) + DEMI_LARGEUR_BINIOU_CM; + 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; + 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 ); + break; + } //deriv correction - derive45 = differenceGD45 - prevDiffGD45; - derive90 = differenceGD90 - prevDiffGD90; + derive45 = positionSurPiste45 - positionSurPiste45Prev; + derive90 = positionSurPiste90 - positionSurPiste90Prev; //integral correction - lastDifferences90[lastDifferenceIndex] = differenceGD90; + lastDifferences90[lastDifferenceIndex] = (p_sectionCourante->consigne_position - positionSurPiste90); integralSum=0; for(int f=0; f<NB_INTEGRAL_SAMPLES; f++) { integralSum+=lastDifferences90[f]; @@ -546,9 +627,12 @@ lastDifferenceIndex = (lastDifferenceIndex + 1)%NB_INTEGRAL_SAMPLES; //application des coefficients - pulseDirection_us = ( ((differenceGD90*2+differenceGD45*4)/8) / p_sectionCourante->coef_p) - + ( derive45 / p_sectionCourante->coef_d) - + (integralSum / p_sectionCourante->coef_i) + pulseDirection_us = 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; //gestioon du dépassement