![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TRR2018 omar
Fork of biniou by
Diff: stateMachines.cpp
- Revision:
- 11:bc24b3ba51a9
- Parent:
- 10:5bf1e6af26c0
- Child:
- 12:51b1b40cc017
--- a/stateMachines.cpp Sat Sep 08 22:24:50 2018 +0000 +++ b/stateMachines.cpp Sun Sep 09 23:14:53 2018 +0000 @@ -1,68 +1,143 @@ #include "states_m.h" - -#define NB_ECHANTILLONS_IR 4 -#define NB_ECHANTILLONS_LIDAR 4 - -static int mode_courant = ; // mettre en define pour la compilation ? ou régler via télécommande ? - -uint16_t dist_murGF[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche pour moyenne -uint16_t dist_murGL[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne -uint16_t dist_murDF[NB_ECHANTILLONS_IR];//buffer tournant ir avant droit pour moyenne -uint16_t dist_murDL[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit pour moyenne +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 +#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 int dist_obstacle_lidar[NB_ECHANTILLONS_LIDAR];//buffer tournant int index_fifo_ir = 0;//pour géreer le buffer tournant int index_fifo_lidar = 0; +int tmpDist; +//sections +s_Section p_section1; -int tachy_cmps; //Capteurs -AnalogIn anaGF(A0);//capteur ir avant gauche 45 deg -AnalogIn anaGL(A1);//capteur ir coté gauche -AnalogIn anaDF(A2);//capteur ir avant droit 45deg -AnalogIn anaDL(A3);//capteur ir coté droit +AnalogIn anaG90(A0);//capteur ir coté gauche +AnalogIn anaD90(A1);//capteur ir coté droit +AnalogIn anaG45(A2);//capteur ir avant gauche 45 deg +AnalogIn anaD45(A3);//capteur ir avant droit 45deg +#ifdef DLVV AnalogIn anaDlvvG(A4);//capteur ir avant droit 10 deg AnalogIn anaDlvvD(A5);//capteur ir coté droit 10 deg AnalogIn anaDlvvFront(A6);//capteur ir avant -Serial device(A4,A5); // tx, rx capteur longue portée +#endif Ticker TIMX(TIMX);//compteur de tours Digital InterruptSW(D0);//interrupt feu DLVV -int lidarStrength; -int lidarDist; + +//LIDAR +Serial serialLidar(A4,A5); // tx, rx + +int distLidar;// LiDAR actually measured distance value +int strengthLidar;// LiDAR signal strength +int check;// check numerical value storage +int i,j; +int uart[9];// store data measured by LiDAR +const int HEADER=0x59;// data package frame header + +//SPEED +int maxSpeed_cmps = 0; +int tachySpeed_cmps = 0; +int tachyStepsRegister = 0; +int tachySectionDist_cm = 0; +float ms_pwmSpeedPulse = 0.0015;//IDLE //Etats -SECTION_ST st_CurrentSection; -OBSTACLE_ST st_obstacleDet; MUR_ST st_murs; +SECTION_ST st_currentSection; +MAX_SPEED_ST st_maxSpeed; THROTTLE_ST st_thro; +#ifdef DLVV +OBSTACLE_ST st_obstacle; +#endif + +s_Section* p_sectionCourante = NULL; //time monitoring -Timer temps;// temps.start()/stop()/sec: read()/ms: read_ms()/µs: read_us() +Timer timeSinceStart;// temps.start()/stop()/sec: read()/ms: read_ms()/µs: read_us() +Timer timerSinceGetTachy; + +void getTachySpeed() +{ + //tachySteps = VALEUR_DU_PIN; + tachySectionDist_cm += tachyStepsRegister * 4; + tachySpeed_cmps = (tachyStepsRegister * 40000)/timerSinceGetTachy.read_us(); + tachyStepsRegister=0; + timerSinceGetTachy.reset(); + timerSinceGetTachy.start(); + return; +} - +int getDistMoy(int* tab,int size) +{ + int sumMoy = 0; + for(int k=0; k<size;k++) + { + sumMoy+=tab[k]; + } +#if (NB_ECHANTILLONS_IR == 4 && NB_ECHANTILLONS_LIDAR == 4) + return sumMoy >> 2; +#else + return sumMoy/size; +#endif +} //########## INIT STATES MACHINES ########## -void murs_init(void) +void mursInit(void) { - st_murs=MUR_GD_OK; + st_murs=EQUILIBRAGE_REPULSIF; return; } -void obstacle_init(void) +#ifdef DLVV +void obstacleInit(void) { - st_obstacleDet=LONG_RANGE_CLEAR; + st_speedLimit=FRONT_CLEAR; return; } -void section_init(void) +#endif +void sectionInit(void) { - temps.reset(); - temps.start(); - st_CurrentSection=ARRET; + timeSinceStart.reset(); + timeSinceStart.start(); + st_currentSection=ARRET; + p_sectionCourante->nextSection=&p_section1; + timerSinceGetTachy.start(); + getTachySpeed();//to reset + tachySectionDist_cm = 0; + tachyStepsRegister = 0; + + //section de test + p_section1.nextSection = NULL; + p_section1.targetSpeed_cmps = 50; + p_section1.brakingCoefficient = + p_section1.ms_accel = 0.00005; + p_section1.ms_decel = 0.0001; + p_section1.lidarWarningDist_cm = 200; + p_section1.slowSpeed_cmps = 15; + p_section1.lng_section_cm = 1000;//10m + p_section1.coef_p = 1; + p_section1.coef_i = 0; + p_section1.coef_d = 0; return; } -void throttle_init(void) + +void maxSpeedInit(void) +{ + st_maxSpeed=SPEED_MAX; + maxSpeed_cmps= p_sectionCourante->targetSpeed_cmps; + serialLidar.baud(115200); + return; +} + +void throttleInit(void) { st_thro = AT_SPEED; return; @@ -70,55 +145,258 @@ //########## UPDATE STATES ########## -void murs_update(void) +void mursUpdate(void) { - MUR_ST st_tmp_murs; - dist_murGF=anaGF.read_u16(); - dist_murGL=anaGL.read_u16(); - dist_murDF=anaDF.read_u16(); - dist_murDL=anaDL.read_u16(); + MUR_ST st_tmpMurs; + //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(); + index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR; - if(1) - { - st_tmp_murs = MUR_GD_OK; - }else - { - st_tmp_murs = MURS_ABSENTS; + switch (st_murs) { + case EQUILIBRAGE_REPULSIF: + tmpDist = getDistMoy(distMurG90,NB_ECHANTILLONS_IR); + if( tmpDist <= IR_DEADZONE_U16_22cm) + { + st_tmpMurs = ATTRACTIF_D; + }else if(tmpDist <= IR_DEADZONE_U16_22cm) + { + st_tmpMurs = ATTRACTIF_G; + }else + { + st_tmpMurs = EQUILIBRAGE_REPULSIF; + } + break; + case ATTRACTIF_D: + tmpDist= getDistMoy(distMurD90,NB_ECHANTILLONS_IR); + if( tmpDist >= IR_FAR_U16_105cm) + { + st_tmpMurs = ATTRACTIF_D; + }else + { + st_tmpMurs = EQUILIBRAGE_REPULSIF; + } + break; + case ATTRACTIF_G: + tmpDist= getDistMoy(distMurG90,NB_ECHANTILLONS_IR); + if(tmpDist >= IR_FAR_U16_105cm) + { + st_tmpMurs = ATTRACTIF_G; + }else + { + st_tmpMurs = EQUILIBRAGE_REPULSIF; + } + break; + default: + return; + break; } - - st_murs = st_tmp_murs; + st_murs = st_tmpMurs; return; } -void obstacle_update(void) +#ifdef DLVV +void obstacleUpdate(void) { return; } -void section_update(void) +#endif + +void sectionUpdate(void) { + SECTION_ST st_tmpSection; + switch (st_currentSection) { + case RUNNING_SECTION: + if(tachySectionDist_cm > p_sectionCourante->lng_section_cm)//on pourrait rajouter un test lidar + { + st_tmpSection = LOADING_SECTION; + } + break; + case LOADING_SECTION: + if(p_sectionCourante->nextSection != NULL) + { + 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; } -void throttle_update(void) +void maxSpeedUpdate(void) { + MAX_SPEED_ST st_tmpMaxSpeed; + i=0; + if (serialLidar.readable()) { //check whether the serial port has data input + if(serialLidar.getc()==HEADER) { // determine data package frame header 0x59 + uart[0]=HEADER; + if(serialLidar.getc()==HEADER) { //determine data package frame header 0x59 + uart[1]=HEADER; + for(i=2; i<9; i++) { // store data to array + uart[i]=serialLidar.getc(); + } + check=uart[0]+uart[1]+uart[2]+uart[3]+uart[4]+uart[5]+uart[6]+uart[7]; + if(uart[8]==(check&0xff)) { // check the received data as per protocols + distLidar=uart[2]+uart[3]*256;// calculate distance value + strengthLidar=uart[4]+uart[5]*256;// calculate signal strength value + } + } + } + } + + if( strengthLidar > (LIDAR_STRENGTH_THRESOLD + LIDAR_STRENGH_DELTA) ) + { + if(distLidar > 30 && distLidar < p_sectionCourante->lidarWarningDist_cm) + { + st_tmpMaxSpeed = SPEED_VARIABLE; + }else + { + st_tmpMaxSpeed = SPEED_LIMITED; + } + }else if(strengthLidar < (LIDAR_STRENGTH_THRESOLD - LIDAR_STRENGH_DELTA)) + { + st_tmpMaxSpeed = SPEED_MAX; + } + else{ + return;//pour ne pas changer sans arret d'etats lorsque strengthLidar est à la limite du seuil + } + + + st_maxSpeed = st_tmpMaxSpeed; + return; +} + +void throttleUpdate(void) +{ + THROTTLE_ST st_tmpThro; + + switch (st_thro) { + case UNDER_SPEED: + if( tachySpeed_cmps > maxSpeed_cmps ) + { + st_tmpThro = AT_SPEED; + }else + { + return; + } + break; + case OVER_SPEED: + if( tachySpeed_cmps < maxSpeed_cmps ) + { + st_tmpThro = AT_SPEED; + }else + { + return; + } + break; + case AT_SPEED: + if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS) ) + { + st_tmpThro = OVER_SPEED; + } else if( tachySpeed_cmps < (maxSpeed_cmps - SPEED_DELTA_CMPS) ) + { + st_tmpThro = UNDER_SPEED; + }else + { + return; + } + break; + default: + break; + } + + st_thro = st_tmpThro; return; } //########## OUTPUT STATES ########## -void murs_output(void) -{ +//updating output parameters +void mursOutput(void) +{ + switch (st_murs) { + case EQUILIBRAGE_REPULSIF: + break; + case ATTRACTIF_G: + break; + case ATTRACTIF_D: + break; + default: + break; + } return; } -void obstacle_output(void) +#ifdef DLVV +void obstacleOutput(void) { return; } -void section_output(void) +#endif +void sectionOutput(void) { + + switch (st_currentSection) { + case RUNNING_SECTION: + break; + case LOADING_SECTION: + p_sectionCourante=p_sectionCourante->nextSection; + tachySectionDist_cm = 0; + break; + case ARRET: + wait(10);//on est à l'arret + break; + default: + break; + } return; } -void throttle_output(void) + +void maxSpeedOutput(void) { + switch(st_maxSpeed) + { + case SPEED_LIMITED: + maxSpeed_cmps = p_sectionCourante->slowSpeed_cmps; + break; + case SPEED_VARIABLE: + maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps * (distLidar/(distLidar+p_sectionCourante->brakingCoefficient)); + break; + default: + maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps; + break; + } return; } + +void throttleOutput(void) +{ + switch (st_thro) { + case UNDER_SPEED: + ms_pwmSpeedPulse += p_sectionCourante->ms_accel; + break; + case OVER_SPEED: + ms_pwmSpeedPulse -= p_sectionCourante->ms_decel; + break; + case AT_SPEED: + default: + break; + } + return; +}