Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@11:bc24b3ba51a9, 2018-09-09 (annotated)

Committer:
GaspardD
Date:
Sun Sep 09 23:14:53 2018 +0000
Revision:
11:bc24b3ba51a9
Parent:
10:5bf1e6af26c0
Child:
12:51b1b40cc017
avancement

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GaspardD 8:1d8c3ca5e508 1 #include "states_m.h"
GaspardD 8:1d8c3ca5e508 2
GaspardD 11:bc24b3ba51a9 3 uint16_t distMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne
GaspardD 11:bc24b3ba51a9 4 uint16_t distMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit pour moyenne
GaspardD 11:bc24b3ba51a9 5 uint16_t distMurG45[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 45deg pour moyenne
GaspardD 11:bc24b3ba51a9 6 uint16_t distMurD45[NB_ECHANTILLONS_IR];//buffer tournant ir avant droit 45deg pour moyenne
GaspardD 11:bc24b3ba51a9 7 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 8 uint16_t distMurG10[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 10deg pour moyenne
GaspardD 11:bc24b3ba51a9 9 uint16_t distMurD10[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit 10deg pour moyenne
GaspardD 11:bc24b3ba51a9 10 uint16_t distMurFront[NB_ECHANTILLONS_IR];//buffer tournant ir front
GaspardD 11:bc24b3ba51a9 11 #endif
GaspardD 8:1d8c3ca5e508 12 int dist_obstacle_lidar[NB_ECHANTILLONS_LIDAR];//buffer tournant
GaspardD 8:1d8c3ca5e508 13 int index_fifo_ir = 0;//pour géreer le buffer tournant
GaspardD 8:1d8c3ca5e508 14 int index_fifo_lidar = 0;
GaspardD 11:bc24b3ba51a9 15 int tmpDist;
GaspardD 11:bc24b3ba51a9 16 //sections
GaspardD 11:bc24b3ba51a9 17 s_Section p_section1;
GaspardD 2:fd0ffe46a87d 18
GaspardD 8:1d8c3ca5e508 19
GaspardD 8:1d8c3ca5e508 20 //Capteurs
GaspardD 8:1d8c3ca5e508 21
GaspardD 11:bc24b3ba51a9 22 AnalogIn anaG90(A0);//capteur ir coté gauche
GaspardD 11:bc24b3ba51a9 23 AnalogIn anaD90(A1);//capteur ir coté droit
GaspardD 11:bc24b3ba51a9 24 AnalogIn anaG45(A2);//capteur ir avant gauche 45 deg
GaspardD 11:bc24b3ba51a9 25 AnalogIn anaD45(A3);//capteur ir avant droit 45deg
GaspardD 11:bc24b3ba51a9 26 #ifdef DLVV
GaspardD 9:747fd4a80225 27 AnalogIn anaDlvvG(A4);//capteur ir avant droit 10 deg
GaspardD 9:747fd4a80225 28 AnalogIn anaDlvvD(A5);//capteur ir coté droit 10 deg
GaspardD 9:747fd4a80225 29 AnalogIn anaDlvvFront(A6);//capteur ir avant
GaspardD 11:bc24b3ba51a9 30 #endif
GaspardD 9:747fd4a80225 31 Ticker TIMX(TIMX);//compteur de tours
GaspardD 9:747fd4a80225 32 Digital InterruptSW(D0);//interrupt feu DLVV
GaspardD 2:fd0ffe46a87d 33
GaspardD 11:bc24b3ba51a9 34
GaspardD 11:bc24b3ba51a9 35 //LIDAR
GaspardD 11:bc24b3ba51a9 36 Serial serialLidar(A4,A5); // tx, rx
GaspardD 11:bc24b3ba51a9 37
GaspardD 11:bc24b3ba51a9 38 int distLidar;// LiDAR actually measured distance value
GaspardD 11:bc24b3ba51a9 39 int strengthLidar;// LiDAR signal strength
GaspardD 11:bc24b3ba51a9 40 int check;// check numerical value storage
GaspardD 11:bc24b3ba51a9 41 int i,j;
GaspardD 11:bc24b3ba51a9 42 int uart[9];// store data measured by LiDAR
GaspardD 11:bc24b3ba51a9 43 const int HEADER=0x59;// data package frame header
GaspardD 11:bc24b3ba51a9 44
GaspardD 11:bc24b3ba51a9 45 //SPEED
GaspardD 11:bc24b3ba51a9 46 int maxSpeed_cmps = 0;
GaspardD 11:bc24b3ba51a9 47 int tachySpeed_cmps = 0;
GaspardD 11:bc24b3ba51a9 48 int tachyStepsRegister = 0;
GaspardD 11:bc24b3ba51a9 49 int tachySectionDist_cm = 0;
GaspardD 11:bc24b3ba51a9 50 float ms_pwmSpeedPulse = 0.0015;//IDLE
GaspardD 8:1d8c3ca5e508 51
GaspardD 8:1d8c3ca5e508 52 //Etats
GaspardD 8:1d8c3ca5e508 53 MUR_ST st_murs;
GaspardD 11:bc24b3ba51a9 54 SECTION_ST st_currentSection;
GaspardD 11:bc24b3ba51a9 55 MAX_SPEED_ST st_maxSpeed;
GaspardD 8:1d8c3ca5e508 56 THROTTLE_ST st_thro;
GaspardD 11:bc24b3ba51a9 57 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 58 OBSTACLE_ST st_obstacle;
GaspardD 11:bc24b3ba51a9 59 #endif
GaspardD 11:bc24b3ba51a9 60
GaspardD 11:bc24b3ba51a9 61 s_Section* p_sectionCourante = NULL;
GaspardD 8:1d8c3ca5e508 62
GaspardD 8:1d8c3ca5e508 63 //time monitoring
GaspardD 8:1d8c3ca5e508 64
GaspardD 11:bc24b3ba51a9 65 Timer timeSinceStart;// temps.start()/stop()/sec: read()/ms: read_ms()/µs: read_us()
GaspardD 11:bc24b3ba51a9 66 Timer timerSinceGetTachy;
GaspardD 11:bc24b3ba51a9 67
GaspardD 11:bc24b3ba51a9 68 void getTachySpeed()
GaspardD 11:bc24b3ba51a9 69 {
GaspardD 11:bc24b3ba51a9 70 //tachySteps = VALEUR_DU_PIN;
GaspardD 11:bc24b3ba51a9 71 tachySectionDist_cm += tachyStepsRegister * 4;
GaspardD 11:bc24b3ba51a9 72 tachySpeed_cmps = (tachyStepsRegister * 40000)/timerSinceGetTachy.read_us();
GaspardD 11:bc24b3ba51a9 73 tachyStepsRegister=0;
GaspardD 11:bc24b3ba51a9 74 timerSinceGetTachy.reset();
GaspardD 11:bc24b3ba51a9 75 timerSinceGetTachy.start();
GaspardD 11:bc24b3ba51a9 76 return;
GaspardD 11:bc24b3ba51a9 77 }
GaspardD 8:1d8c3ca5e508 78
GaspardD 11:bc24b3ba51a9 79 int getDistMoy(int* tab,int size)
GaspardD 11:bc24b3ba51a9 80 {
GaspardD 11:bc24b3ba51a9 81 int sumMoy = 0;
GaspardD 11:bc24b3ba51a9 82 for(int k=0; k<size;k++)
GaspardD 11:bc24b3ba51a9 83 {
GaspardD 11:bc24b3ba51a9 84 sumMoy+=tab[k];
GaspardD 11:bc24b3ba51a9 85 }
GaspardD 11:bc24b3ba51a9 86 #if (NB_ECHANTILLONS_IR == 4 && NB_ECHANTILLONS_LIDAR == 4)
GaspardD 11:bc24b3ba51a9 87 return sumMoy >> 2;
GaspardD 11:bc24b3ba51a9 88 #else
GaspardD 11:bc24b3ba51a9 89 return sumMoy/size;
GaspardD 11:bc24b3ba51a9 90 #endif
GaspardD 11:bc24b3ba51a9 91 }
GaspardD 2:fd0ffe46a87d 92
GaspardD 8:1d8c3ca5e508 93 //########## INIT STATES MACHINES ##########
GaspardD 11:bc24b3ba51a9 94 void mursInit(void)
GaspardD 8:1d8c3ca5e508 95 {
GaspardD 11:bc24b3ba51a9 96 st_murs=EQUILIBRAGE_REPULSIF;
GaspardD 8:1d8c3ca5e508 97 return;
GaspardD 8:1d8c3ca5e508 98 }
GaspardD 11:bc24b3ba51a9 99 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 100 void obstacleInit(void)
GaspardD 8:1d8c3ca5e508 101 {
GaspardD 11:bc24b3ba51a9 102 st_speedLimit=FRONT_CLEAR;
GaspardD 8:1d8c3ca5e508 103 return;
GaspardD 8:1d8c3ca5e508 104 }
GaspardD 11:bc24b3ba51a9 105 #endif
GaspardD 11:bc24b3ba51a9 106 void sectionInit(void)
GaspardD 2:fd0ffe46a87d 107 {
GaspardD 11:bc24b3ba51a9 108 timeSinceStart.reset();
GaspardD 11:bc24b3ba51a9 109 timeSinceStart.start();
GaspardD 11:bc24b3ba51a9 110 st_currentSection=ARRET;
GaspardD 11:bc24b3ba51a9 111 p_sectionCourante->nextSection=&p_section1;
GaspardD 11:bc24b3ba51a9 112 timerSinceGetTachy.start();
GaspardD 11:bc24b3ba51a9 113 getTachySpeed();//to reset
GaspardD 11:bc24b3ba51a9 114 tachySectionDist_cm = 0;
GaspardD 11:bc24b3ba51a9 115 tachyStepsRegister = 0;
GaspardD 11:bc24b3ba51a9 116
GaspardD 11:bc24b3ba51a9 117 //section de test
GaspardD 11:bc24b3ba51a9 118 p_section1.nextSection = NULL;
GaspardD 11:bc24b3ba51a9 119 p_section1.targetSpeed_cmps = 50;
GaspardD 11:bc24b3ba51a9 120 p_section1.brakingCoefficient =
GaspardD 11:bc24b3ba51a9 121 p_section1.ms_accel = 0.00005;
GaspardD 11:bc24b3ba51a9 122 p_section1.ms_decel = 0.0001;
GaspardD 11:bc24b3ba51a9 123 p_section1.lidarWarningDist_cm = 200;
GaspardD 11:bc24b3ba51a9 124 p_section1.slowSpeed_cmps = 15;
GaspardD 11:bc24b3ba51a9 125 p_section1.lng_section_cm = 1000;//10m
GaspardD 11:bc24b3ba51a9 126 p_section1.coef_p = 1;
GaspardD 11:bc24b3ba51a9 127 p_section1.coef_i = 0;
GaspardD 11:bc24b3ba51a9 128 p_section1.coef_d = 0;
GaspardD 8:1d8c3ca5e508 129 return;
GaspardD 8:1d8c3ca5e508 130 }
GaspardD 11:bc24b3ba51a9 131
GaspardD 11:bc24b3ba51a9 132 void maxSpeedInit(void)
GaspardD 11:bc24b3ba51a9 133 {
GaspardD 11:bc24b3ba51a9 134 st_maxSpeed=SPEED_MAX;
GaspardD 11:bc24b3ba51a9 135 maxSpeed_cmps= p_sectionCourante->targetSpeed_cmps;
GaspardD 11:bc24b3ba51a9 136 serialLidar.baud(115200);
GaspardD 11:bc24b3ba51a9 137 return;
GaspardD 11:bc24b3ba51a9 138 }
GaspardD 11:bc24b3ba51a9 139
GaspardD 11:bc24b3ba51a9 140 void throttleInit(void)
GaspardD 8:1d8c3ca5e508 141 {
GaspardD 8:1d8c3ca5e508 142 st_thro = AT_SPEED;
GaspardD 8:1d8c3ca5e508 143 return;
GaspardD 8:1d8c3ca5e508 144 }
GaspardD 2:fd0ffe46a87d 145
GaspardD 8:1d8c3ca5e508 146
GaspardD 8:1d8c3ca5e508 147 //########## UPDATE STATES ##########
GaspardD 11:bc24b3ba51a9 148 void mursUpdate(void)
GaspardD 2:fd0ffe46a87d 149 {
GaspardD 11:bc24b3ba51a9 150 MUR_ST st_tmpMurs;
GaspardD 11:bc24b3ba51a9 151 //lectures
GaspardD 11:bc24b3ba51a9 152 distMurG90[index_fifo_ir]=anaG90.read_u16();
GaspardD 11:bc24b3ba51a9 153 distMurD90[index_fifo_ir]=anaD90.read_u16();
GaspardD 11:bc24b3ba51a9 154 distMurG45[index_fifo_ir]=anaG45.read_u16();
GaspardD 11:bc24b3ba51a9 155 distMurD45[index_fifo_ir]=anaD45.read_u16();
GaspardD 11:bc24b3ba51a9 156 index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR;
GaspardD 8:1d8c3ca5e508 157
GaspardD 11:bc24b3ba51a9 158 switch (st_murs) {
GaspardD 11:bc24b3ba51a9 159 case EQUILIBRAGE_REPULSIF:
GaspardD 11:bc24b3ba51a9 160 tmpDist = getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
GaspardD 11:bc24b3ba51a9 161 if( tmpDist <= IR_DEADZONE_U16_22cm)
GaspardD 11:bc24b3ba51a9 162 {
GaspardD 11:bc24b3ba51a9 163 st_tmpMurs = ATTRACTIF_D;
GaspardD 11:bc24b3ba51a9 164 }else if(tmpDist <= IR_DEADZONE_U16_22cm)
GaspardD 11:bc24b3ba51a9 165 {
GaspardD 11:bc24b3ba51a9 166 st_tmpMurs = ATTRACTIF_G;
GaspardD 11:bc24b3ba51a9 167 }else
GaspardD 11:bc24b3ba51a9 168 {
GaspardD 11:bc24b3ba51a9 169 st_tmpMurs = EQUILIBRAGE_REPULSIF;
GaspardD 11:bc24b3ba51a9 170 }
GaspardD 11:bc24b3ba51a9 171 break;
GaspardD 11:bc24b3ba51a9 172 case ATTRACTIF_D:
GaspardD 11:bc24b3ba51a9 173 tmpDist= getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
GaspardD 11:bc24b3ba51a9 174 if( tmpDist >= IR_FAR_U16_105cm)
GaspardD 11:bc24b3ba51a9 175 {
GaspardD 11:bc24b3ba51a9 176 st_tmpMurs = ATTRACTIF_D;
GaspardD 11:bc24b3ba51a9 177 }else
GaspardD 11:bc24b3ba51a9 178 {
GaspardD 11:bc24b3ba51a9 179 st_tmpMurs = EQUILIBRAGE_REPULSIF;
GaspardD 11:bc24b3ba51a9 180 }
GaspardD 11:bc24b3ba51a9 181 break;
GaspardD 11:bc24b3ba51a9 182 case ATTRACTIF_G:
GaspardD 11:bc24b3ba51a9 183 tmpDist= getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
GaspardD 11:bc24b3ba51a9 184 if(tmpDist >= IR_FAR_U16_105cm)
GaspardD 11:bc24b3ba51a9 185 {
GaspardD 11:bc24b3ba51a9 186 st_tmpMurs = ATTRACTIF_G;
GaspardD 11:bc24b3ba51a9 187 }else
GaspardD 11:bc24b3ba51a9 188 {
GaspardD 11:bc24b3ba51a9 189 st_tmpMurs = EQUILIBRAGE_REPULSIF;
GaspardD 11:bc24b3ba51a9 190 }
GaspardD 11:bc24b3ba51a9 191 break;
GaspardD 11:bc24b3ba51a9 192 default:
GaspardD 11:bc24b3ba51a9 193 return;
GaspardD 11:bc24b3ba51a9 194 break;
GaspardD 8:1d8c3ca5e508 195 }
GaspardD 11:bc24b3ba51a9 196 st_murs = st_tmpMurs;
GaspardD 8:1d8c3ca5e508 197 return;
GaspardD 8:1d8c3ca5e508 198 }
GaspardD 11:bc24b3ba51a9 199 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 200 void obstacleUpdate(void)
GaspardD 8:1d8c3ca5e508 201 {
GaspardD 8:1d8c3ca5e508 202 return;
GaspardD 8:1d8c3ca5e508 203 }
GaspardD 11:bc24b3ba51a9 204 #endif
GaspardD 11:bc24b3ba51a9 205
GaspardD 11:bc24b3ba51a9 206 void sectionUpdate(void)
GaspardD 8:1d8c3ca5e508 207 {
GaspardD 11:bc24b3ba51a9 208 SECTION_ST st_tmpSection;
GaspardD 11:bc24b3ba51a9 209 switch (st_currentSection) {
GaspardD 11:bc24b3ba51a9 210 case RUNNING_SECTION:
GaspardD 11:bc24b3ba51a9 211 if(tachySectionDist_cm > p_sectionCourante->lng_section_cm)//on pourrait rajouter un test lidar
GaspardD 11:bc24b3ba51a9 212 {
GaspardD 11:bc24b3ba51a9 213 st_tmpSection = LOADING_SECTION;
GaspardD 11:bc24b3ba51a9 214 }
GaspardD 11:bc24b3ba51a9 215 break;
GaspardD 11:bc24b3ba51a9 216 case LOADING_SECTION:
GaspardD 11:bc24b3ba51a9 217 if(p_sectionCourante->nextSection != NULL)
GaspardD 11:bc24b3ba51a9 218 {
GaspardD 11:bc24b3ba51a9 219 st_tmpSection = RUNNING_SECTION;
GaspardD 11:bc24b3ba51a9 220 }else
GaspardD 11:bc24b3ba51a9 221 {
GaspardD 11:bc24b3ba51a9 222 st_tmpSection=ARRET;
GaspardD 11:bc24b3ba51a9 223 }
GaspardD 11:bc24b3ba51a9 224 break;
GaspardD 11:bc24b3ba51a9 225 case ARRET:
GaspardD 11:bc24b3ba51a9 226 if(p_sectionCourante != NULL)
GaspardD 11:bc24b3ba51a9 227 {
GaspardD 11:bc24b3ba51a9 228 st_tmpSection = RUNNING_SECTION;
GaspardD 11:bc24b3ba51a9 229 }
GaspardD 11:bc24b3ba51a9 230 else
GaspardD 11:bc24b3ba51a9 231 {
GaspardD 11:bc24b3ba51a9 232 return;
GaspardD 11:bc24b3ba51a9 233 }
GaspardD 11:bc24b3ba51a9 234 break;
GaspardD 11:bc24b3ba51a9 235 default:
GaspardD 11:bc24b3ba51a9 236 break;
GaspardD 11:bc24b3ba51a9 237 }
GaspardD 11:bc24b3ba51a9 238 st_currentSection = st_tmpSection;
GaspardD 8:1d8c3ca5e508 239 return;
GaspardD 8:1d8c3ca5e508 240 }
GaspardD 8:1d8c3ca5e508 241
GaspardD 11:bc24b3ba51a9 242 void maxSpeedUpdate(void)
GaspardD 8:1d8c3ca5e508 243 {
GaspardD 11:bc24b3ba51a9 244 MAX_SPEED_ST st_tmpMaxSpeed;
GaspardD 11:bc24b3ba51a9 245 i=0;
GaspardD 11:bc24b3ba51a9 246 if (serialLidar.readable()) { //check whether the serial port has data input
GaspardD 11:bc24b3ba51a9 247 if(serialLidar.getc()==HEADER) { // determine data package frame header 0x59
GaspardD 11:bc24b3ba51a9 248 uart[0]=HEADER;
GaspardD 11:bc24b3ba51a9 249 if(serialLidar.getc()==HEADER) { //determine data package frame header 0x59
GaspardD 11:bc24b3ba51a9 250 uart[1]=HEADER;
GaspardD 11:bc24b3ba51a9 251 for(i=2; i<9; i++) { // store data to array
GaspardD 11:bc24b3ba51a9 252 uart[i]=serialLidar.getc();
GaspardD 11:bc24b3ba51a9 253 }
GaspardD 11:bc24b3ba51a9 254 check=uart[0]+uart[1]+uart[2]+uart[3]+uart[4]+uart[5]+uart[6]+uart[7];
GaspardD 11:bc24b3ba51a9 255 if(uart[8]==(check&0xff)) { // check the received data as per protocols
GaspardD 11:bc24b3ba51a9 256 distLidar=uart[2]+uart[3]*256;// calculate distance value
GaspardD 11:bc24b3ba51a9 257 strengthLidar=uart[4]+uart[5]*256;// calculate signal strength value
GaspardD 11:bc24b3ba51a9 258 }
GaspardD 11:bc24b3ba51a9 259 }
GaspardD 11:bc24b3ba51a9 260 }
GaspardD 11:bc24b3ba51a9 261 }
GaspardD 11:bc24b3ba51a9 262
GaspardD 11:bc24b3ba51a9 263 if( strengthLidar > (LIDAR_STRENGTH_THRESOLD + LIDAR_STRENGH_DELTA) )
GaspardD 11:bc24b3ba51a9 264 {
GaspardD 11:bc24b3ba51a9 265 if(distLidar > 30 && distLidar < p_sectionCourante->lidarWarningDist_cm)
GaspardD 11:bc24b3ba51a9 266 {
GaspardD 11:bc24b3ba51a9 267 st_tmpMaxSpeed = SPEED_VARIABLE;
GaspardD 11:bc24b3ba51a9 268 }else
GaspardD 11:bc24b3ba51a9 269 {
GaspardD 11:bc24b3ba51a9 270 st_tmpMaxSpeed = SPEED_LIMITED;
GaspardD 11:bc24b3ba51a9 271 }
GaspardD 11:bc24b3ba51a9 272 }else if(strengthLidar < (LIDAR_STRENGTH_THRESOLD - LIDAR_STRENGH_DELTA))
GaspardD 11:bc24b3ba51a9 273 {
GaspardD 11:bc24b3ba51a9 274 st_tmpMaxSpeed = SPEED_MAX;
GaspardD 11:bc24b3ba51a9 275 }
GaspardD 11:bc24b3ba51a9 276 else{
GaspardD 11:bc24b3ba51a9 277 return;//pour ne pas changer sans arret d'etats lorsque strengthLidar est à la limite du seuil
GaspardD 11:bc24b3ba51a9 278 }
GaspardD 11:bc24b3ba51a9 279
GaspardD 11:bc24b3ba51a9 280
GaspardD 11:bc24b3ba51a9 281 st_maxSpeed = st_tmpMaxSpeed;
GaspardD 11:bc24b3ba51a9 282 return;
GaspardD 11:bc24b3ba51a9 283 }
GaspardD 11:bc24b3ba51a9 284
GaspardD 11:bc24b3ba51a9 285 void throttleUpdate(void)
GaspardD 11:bc24b3ba51a9 286 {
GaspardD 11:bc24b3ba51a9 287 THROTTLE_ST st_tmpThro;
GaspardD 11:bc24b3ba51a9 288
GaspardD 11:bc24b3ba51a9 289 switch (st_thro) {
GaspardD 11:bc24b3ba51a9 290 case UNDER_SPEED:
GaspardD 11:bc24b3ba51a9 291 if( tachySpeed_cmps > maxSpeed_cmps )
GaspardD 11:bc24b3ba51a9 292 {
GaspardD 11:bc24b3ba51a9 293 st_tmpThro = AT_SPEED;
GaspardD 11:bc24b3ba51a9 294 }else
GaspardD 11:bc24b3ba51a9 295 {
GaspardD 11:bc24b3ba51a9 296 return;
GaspardD 11:bc24b3ba51a9 297 }
GaspardD 11:bc24b3ba51a9 298 break;
GaspardD 11:bc24b3ba51a9 299 case OVER_SPEED:
GaspardD 11:bc24b3ba51a9 300 if( tachySpeed_cmps < maxSpeed_cmps )
GaspardD 11:bc24b3ba51a9 301 {
GaspardD 11:bc24b3ba51a9 302 st_tmpThro = AT_SPEED;
GaspardD 11:bc24b3ba51a9 303 }else
GaspardD 11:bc24b3ba51a9 304 {
GaspardD 11:bc24b3ba51a9 305 return;
GaspardD 11:bc24b3ba51a9 306 }
GaspardD 11:bc24b3ba51a9 307 break;
GaspardD 11:bc24b3ba51a9 308 case AT_SPEED:
GaspardD 11:bc24b3ba51a9 309 if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS) )
GaspardD 11:bc24b3ba51a9 310 {
GaspardD 11:bc24b3ba51a9 311 st_tmpThro = OVER_SPEED;
GaspardD 11:bc24b3ba51a9 312 } else if( tachySpeed_cmps < (maxSpeed_cmps - SPEED_DELTA_CMPS) )
GaspardD 11:bc24b3ba51a9 313 {
GaspardD 11:bc24b3ba51a9 314 st_tmpThro = UNDER_SPEED;
GaspardD 11:bc24b3ba51a9 315 }else
GaspardD 11:bc24b3ba51a9 316 {
GaspardD 11:bc24b3ba51a9 317 return;
GaspardD 11:bc24b3ba51a9 318 }
GaspardD 11:bc24b3ba51a9 319 break;
GaspardD 11:bc24b3ba51a9 320 default:
GaspardD 11:bc24b3ba51a9 321 break;
GaspardD 11:bc24b3ba51a9 322 }
GaspardD 11:bc24b3ba51a9 323
GaspardD 11:bc24b3ba51a9 324 st_thro = st_tmpThro;
GaspardD 8:1d8c3ca5e508 325 return;
GaspardD 8:1d8c3ca5e508 326 }
GaspardD 2:fd0ffe46a87d 327
GaspardD 8:1d8c3ca5e508 328 //########## OUTPUT STATES ##########
GaspardD 11:bc24b3ba51a9 329 //updating output parameters
GaspardD 11:bc24b3ba51a9 330 void mursOutput(void)
GaspardD 11:bc24b3ba51a9 331 {
GaspardD 11:bc24b3ba51a9 332 switch (st_murs) {
GaspardD 11:bc24b3ba51a9 333 case EQUILIBRAGE_REPULSIF:
GaspardD 11:bc24b3ba51a9 334 break;
GaspardD 11:bc24b3ba51a9 335 case ATTRACTIF_G:
GaspardD 11:bc24b3ba51a9 336 break;
GaspardD 11:bc24b3ba51a9 337 case ATTRACTIF_D:
GaspardD 11:bc24b3ba51a9 338 break;
GaspardD 11:bc24b3ba51a9 339 default:
GaspardD 11:bc24b3ba51a9 340 break;
GaspardD 11:bc24b3ba51a9 341 }
GaspardD 8:1d8c3ca5e508 342 return;
GaspardD 8:1d8c3ca5e508 343 }
GaspardD 8:1d8c3ca5e508 344
GaspardD 11:bc24b3ba51a9 345 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 346 void obstacleOutput(void)
GaspardD 8:1d8c3ca5e508 347 {
GaspardD 8:1d8c3ca5e508 348 return;
GaspardD 8:1d8c3ca5e508 349 }
GaspardD 11:bc24b3ba51a9 350 #endif
GaspardD 11:bc24b3ba51a9 351 void sectionOutput(void)
GaspardD 8:1d8c3ca5e508 352 {
GaspardD 11:bc24b3ba51a9 353
GaspardD 11:bc24b3ba51a9 354 switch (st_currentSection) {
GaspardD 11:bc24b3ba51a9 355 case RUNNING_SECTION:
GaspardD 11:bc24b3ba51a9 356 break;
GaspardD 11:bc24b3ba51a9 357 case LOADING_SECTION:
GaspardD 11:bc24b3ba51a9 358 p_sectionCourante=p_sectionCourante->nextSection;
GaspardD 11:bc24b3ba51a9 359 tachySectionDist_cm = 0;
GaspardD 11:bc24b3ba51a9 360 break;
GaspardD 11:bc24b3ba51a9 361 case ARRET:
GaspardD 11:bc24b3ba51a9 362 wait(10);//on est à l'arret
GaspardD 11:bc24b3ba51a9 363 break;
GaspardD 11:bc24b3ba51a9 364 default:
GaspardD 11:bc24b3ba51a9 365 break;
GaspardD 11:bc24b3ba51a9 366 }
GaspardD 8:1d8c3ca5e508 367 return;
GaspardD 8:1d8c3ca5e508 368 }
GaspardD 8:1d8c3ca5e508 369
GaspardD 11:bc24b3ba51a9 370
GaspardD 11:bc24b3ba51a9 371 void maxSpeedOutput(void)
GaspardD 8:1d8c3ca5e508 372 {
GaspardD 11:bc24b3ba51a9 373 switch(st_maxSpeed)
GaspardD 11:bc24b3ba51a9 374 {
GaspardD 11:bc24b3ba51a9 375 case SPEED_LIMITED:
GaspardD 11:bc24b3ba51a9 376 maxSpeed_cmps = p_sectionCourante->slowSpeed_cmps;
GaspardD 11:bc24b3ba51a9 377 break;
GaspardD 11:bc24b3ba51a9 378 case SPEED_VARIABLE:
GaspardD 11:bc24b3ba51a9 379 maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps * (distLidar/(distLidar+p_sectionCourante->brakingCoefficient));
GaspardD 11:bc24b3ba51a9 380 break;
GaspardD 11:bc24b3ba51a9 381 default:
GaspardD 11:bc24b3ba51a9 382 maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps;
GaspardD 11:bc24b3ba51a9 383 break;
GaspardD 11:bc24b3ba51a9 384 }
GaspardD 8:1d8c3ca5e508 385 return;
GaspardD 8:1d8c3ca5e508 386 }
GaspardD 11:bc24b3ba51a9 387
GaspardD 11:bc24b3ba51a9 388 void throttleOutput(void)
GaspardD 11:bc24b3ba51a9 389 {
GaspardD 11:bc24b3ba51a9 390 switch (st_thro) {
GaspardD 11:bc24b3ba51a9 391 case UNDER_SPEED:
GaspardD 11:bc24b3ba51a9 392 ms_pwmSpeedPulse += p_sectionCourante->ms_accel;
GaspardD 11:bc24b3ba51a9 393 break;
GaspardD 11:bc24b3ba51a9 394 case OVER_SPEED:
GaspardD 11:bc24b3ba51a9 395 ms_pwmSpeedPulse -= p_sectionCourante->ms_decel;
GaspardD 11:bc24b3ba51a9 396 break;
GaspardD 11:bc24b3ba51a9 397 case AT_SPEED:
GaspardD 11:bc24b3ba51a9 398 default:
GaspardD 11:bc24b3ba51a9 399 break;
GaspardD 11:bc24b3ba51a9 400 }
GaspardD 11:bc24b3ba51a9 401 return;
GaspardD 11:bc24b3ba51a9 402 }