Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@52:228703200e35, 2018-09-21 (annotated)

Committer:
ohlimi2
Date:
Fri Sep 21 17:29:30 2018 +0000
Revision:
52:228703200e35
Parent:
51:09ecba68d0cf 
version bof

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GaspardD 12:51b1b40cc017 1 #include "stateMachines.h"
ohlimi2 36:bccddd02966a 2 //#if DEBUG >0
GaspardD 13:af9a59ccf60b 3 Serial pc(USBTX, USBRX); // tx, rx
ohlimi2 36:bccddd02966a 4 //#endif
GaspardD 29:fc984fe08ca7 5
GaspardD 29:fc984fe08ca7 6 //*************** declarations ***************
GaspardD 33:88d8f254c0b7 7
GaspardD 33:88d8f254c0b7 8 //time monitoring
GaspardD 23:04d393220daa 9 #ifdef DUMP_SAMPLIG_PERIOD
GaspardD 22:26fc6e6f7a55 10 Timer timerLog;
GaspardD 19:771bf61be276 11 #endif
GaspardD 19:771bf61be276 12
GaspardD 33:88d8f254c0b7 13 Timer timeSinceStart;// temps.start()/stop()/sec: read()/ms: read_ms()/µs: read_us()
GaspardD 39:de3638276b7e 14 Timer timerSinceTachy;
GaspardD 42:3f12252862b9 15 Timer timersinceSerial;
GaspardD 33:88d8f254c0b7 16
GaspardD 29:fc984fe08ca7 17 double distMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne
GaspardD 29:fc984fe08ca7 18 double distMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit pour moyenne
GaspardD 29:fc984fe08ca7 19 double shortDistMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche petites distances pour moyenne
GaspardD 29:fc984fe08ca7 20 double shortDistMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit petites distances pour moyenne
GaspardD 29:fc984fe08ca7 21 double distMurG45[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 45deg pour moyenne
GaspardD 29:fc984fe08ca7 22 double distMurD45[NB_ECHANTILLONS_IR];//buffer tournant ir avant droit 45deg pour moyenne
GaspardD 11:bc24b3ba51a9 23 #ifdef DLVV
GaspardD 33:88d8f254c0b7 24 double distMurG10[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 10deg pour moyenne
GaspardD 33:88d8f254c0b7 25 double distMurD10[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit 10deg pour moyenne
GaspardD 33:88d8f254c0b7 26 double distMurFront[NB_ECHANTILLONS_IR];//buffer tournant ir front
GaspardD 11:bc24b3ba51a9 27 #endif
GaspardD 29:fc984fe08ca7 28 double distMurG90Moy;
GaspardD 29:fc984fe08ca7 29 double distMurD90Moy;
GaspardD 29:fc984fe08ca7 30 double shortDistMurG90Moy;
GaspardD 29:fc984fe08ca7 31 double shortDistMurD90Moy;
GaspardD 29:fc984fe08ca7 32 double trueDistMurG90Moy;
GaspardD 29:fc984fe08ca7 33 double trueDistMurD90Moy;
GaspardD 29:fc984fe08ca7 34 double distMurG45Moy;
GaspardD 29:fc984fe08ca7 35 double distMurD45Moy;
GaspardD 23:04d393220daa 36
GaspardD 12:51b1b40cc017 37 #ifdef DLVV
GaspardD 29:fc984fe08ca7 38 double distMurG10Moy;
GaspardD 29:fc984fe08ca7 39 double distMurD10Moy;
GaspardD 29:fc984fe08ca7 40 double distMurFrontMoy
GaspardD 12:51b1b40cc017 41 #endif
GaspardD 8:1d8c3ca5e508 42 int index_fifo_ir = 0;//pour géreer le buffer tournant
GaspardD 8:1d8c3ca5e508 43 int index_fifo_lidar = 0;
GaspardD 11:bc24b3ba51a9 44 //sections
GaspardD 11:bc24b3ba51a9 45 s_Section p_section1;
GaspardD 39:de3638276b7e 46 s_Section p_section2;
ohlimi2 46:9262b07b0833 47 s_Section p_section3;
GaspardD 2:fd0ffe46a87d 48
GaspardD 15:129f205ff030 49 //PWM Controls
GaspardD 12:51b1b40cc017 50 PwmOut PwmMotor(PB_6); // PWM4 ch1 TIM4
GaspardD 12:51b1b40cc017 51 PwmOut PwmDirection(PB_5); // PWM3 ch2 TIM3
GaspardD 8:1d8c3ca5e508 52
GaspardD 21:de7a0a47f8a3 53 int pulseDirection_us = DIRECTION_PULSE_MIDDLE;
ohlimi2 36:bccddd02966a 54 double pulseDirection_us_temp ;
GaspardD 19:771bf61be276 55 int pulseSpeed_us = INITAL_PULSE_SPEED_US;
GaspardD 12:51b1b40cc017 56 //Capteurs direction
GaspardD 22:26fc6e6f7a55 57 AnalogIn anaG90(CAPT_90_GAUCHE);//capteur ir coté gauche
GaspardD 22:26fc6e6f7a55 58 AnalogIn anaD90(CAPT_90_DROITE);//capteur ir coté droit
GaspardD 29:fc984fe08ca7 59 AnalogIn anaShortG90(CAPT_90_GAUCHE_SHORT);//capteur ir coté gauche short
GaspardD 29:fc984fe08ca7 60 AnalogIn anaShortD90(CAPT_90_DROITE_SHORT);//capteur ir coté droit short
GaspardD 22:26fc6e6f7a55 61 AnalogIn anaG45(CAPT_45_GAUCHE);//capteur ir avant gauche 45 deg
GaspardD 22:26fc6e6f7a55 62 AnalogIn anaD45(CAPT_45_DROITE);//capteur ir avant droit 45deg
GaspardD 11:bc24b3ba51a9 63 #ifdef DLVV
GaspardD 22:26fc6e6f7a55 64 AnalogIn anaDlvvG(CAPT_10_GAUCHE);//capteur ir avant droit 10 deg
GaspardD 22:26fc6e6f7a55 65 AnalogIn anaDlvvD(CAPT_10_DROITE);//capteur ir coté droit 10 deg
GaspardD 22:26fc6e6f7a55 66 AnalogIn anaDlvvFront(CAPT_DEVANT);//capteur ir avant
GaspardD 11:bc24b3ba51a9 67 #endif
GaspardD 12:51b1b40cc017 68
GaspardD 29:fc984fe08ca7 69 //piste
ohlimi2 36:bccddd02966a 70 double largeurPiste90 = 150.0;
ohlimi2 36:bccddd02966a 71 double largeurPiste45 = 150.0;
GaspardD 29:fc984fe08ca7 72 double positionSurPiste90 = 75.0;
GaspardD 29:fc984fe08ca7 73 double positionSurPiste90Prev = positionSurPiste90;
GaspardD 29:fc984fe08ca7 74 double positionSurPiste45 = 75.0;
GaspardD 29:fc984fe08ca7 75 double positionSurPiste45Prev = positionSurPiste45;
GaspardD 29:fc984fe08ca7 76
ohlimi2 36:bccddd02966a 77 double derive45,derive90;
GaspardD 21:de7a0a47f8a3 78 int lastDifferences90[NB_INTEGRAL_SAMPLES] = {0};//for integral correction
GaspardD 21:de7a0a47f8a3 79 int lastDifferenceIndex = 0;
GaspardD 21:de7a0a47f8a3 80 int integralSum;
GaspardD 12:51b1b40cc017 81
GaspardD 12:51b1b40cc017 82 //Capteur vitesse
GaspardD 12:51b1b40cc017 83 InterruptIn it_tachymeter(PA_11);
GaspardD 2:fd0ffe46a87d 84
GaspardD 11:bc24b3ba51a9 85
GaspardD 11:bc24b3ba51a9 86 //LIDAR
GaspardD 12:51b1b40cc017 87 Serial serialLidar(PC_10,PC_11); // tx, rx
GaspardD 11:bc24b3ba51a9 88
ohlimi2 52:228703200e35 89 Serial serialDebug(PC_12,PD_2); // tx, rx
ohlimi2 52:228703200e35 90
GaspardD 11:bc24b3ba51a9 91 int distLidar;// LiDAR actually measured distance value
GaspardD 23:04d393220daa 92 int distLidarPrev;
GaspardD 11:bc24b3ba51a9 93 int strengthLidar;// LiDAR signal strength
GaspardD 23:04d393220daa 94 int strengthLidarPrev;
GaspardD 11:bc24b3ba51a9 95 int check;// check numerical value storage
GaspardD 12:51b1b40cc017 96 int i;
GaspardD 11:bc24b3ba51a9 97 int uart[9];// store data measured by LiDAR
GaspardD 11:bc24b3ba51a9 98 const int HEADER=0x59;// data package frame header
GaspardD 11:bc24b3ba51a9 99
GaspardD 11:bc24b3ba51a9 100 //SPEED
GaspardD 29:fc984fe08ca7 101 double maxSpeed_cmps = 0;
ohlimi2 36:bccddd02966a 102 double tachySpeed_cmps = 0; //en cm/s
GaspardD 39:de3638276b7e 103 double tachyStepsRegister = 0;
GaspardD 29:fc984fe08ca7 104 double tachySectionDist_cm = 0;
ohlimi2 36:bccddd02966a 105 double tachyTotalDist_cm = 0.0;
GaspardD 8:1d8c3ca5e508 106
GaspardD 27:f8c3f1524a64 107 #ifdef FREINAGE_ADAPTATIF
GaspardD 24:698fefbbee00 108 Timer brakingTimer;
GaspardD 24:698fefbbee00 109 int brakingDurationNeeded_us = 0;
GaspardD 27:f8c3f1524a64 110 #endif
GaspardD 24:698fefbbee00 111
GaspardD 8:1d8c3ca5e508 112 //Etats
GaspardD 8:1d8c3ca5e508 113 MUR_ST st_murs;
GaspardD 21:de7a0a47f8a3 114 MUR_ST st_tmpMurs;
GaspardD 11:bc24b3ba51a9 115 SECTION_ST st_currentSection;
GaspardD 21:de7a0a47f8a3 116 SECTION_ST st_tmpSection;
GaspardD 11:bc24b3ba51a9 117 MAX_SPEED_ST st_maxSpeed;
GaspardD 21:de7a0a47f8a3 118 MAX_SPEED_ST st_tmpMaxSpeed;
GaspardD 8:1d8c3ca5e508 119 THROTTLE_ST st_thro;
GaspardD 21:de7a0a47f8a3 120 THROTTLE_ST st_tmpThro;
GaspardD 11:bc24b3ba51a9 121 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 122 OBSTACLE_ST st_obstacle;
GaspardD 11:bc24b3ba51a9 123 #endif
GaspardD 11:bc24b3ba51a9 124
GaspardD 11:bc24b3ba51a9 125 s_Section* p_sectionCourante = NULL;
GaspardD 44:69751c05a8f3 126 bool dumped = false;
GaspardD 8:1d8c3ca5e508 127
GaspardD 19:771bf61be276 128
GaspardD 19:771bf61be276 129 // +++++++++++++++++++++++++++++++++++++++++++ SAMPLING +++++++++++++++++++++++++++++++++++++++++++
GaspardD 19:771bf61be276 130 #ifdef SAMPLING
GaspardD 19:771bf61be276 131 s_Sample history[TAILLE_SAMPLES];
GaspardD 19:771bf61be276 132 int indexSample = 0;
GaspardD 19:771bf61be276 133 void initSamples(void)
GaspardD 19:771bf61be276 134 {
GaspardD 19:771bf61be276 135 for(int m=0; m<TAILLE_SAMPLES; m++) {
GaspardD 19:771bf61be276 136 history[m].states.murs_dlvv = '0';
GaspardD 19:771bf61be276 137 history[m].states.section = '0';
GaspardD 19:771bf61be276 138 history[m].states.maxSpeed = '0';
GaspardD 19:771bf61be276 139 history[m].states.throttle = '0';
GaspardD 19:771bf61be276 140 history[m].time = 0;
GaspardD 29:fc984fe08ca7 141 history[m].position45 = 0.0;
GaspardD 29:fc984fe08ca7 142 history[m].position90 = 0.0;
GaspardD 29:fc984fe08ca7 143 history[m].largeurPiste = 0.0;
GaspardD 29:fc984fe08ca7 144 history[m].dist = 0.0;
GaspardD 21:de7a0a47f8a3 145 history[m].pwm_thro_us = 0;
GaspardD 21:de7a0a47f8a3 146 history[m].pwm_dir_us = 0;
GaspardD 23:04d393220daa 147 history[m].distLidar = 0;
GaspardD 23:04d393220daa 148 history[m].strLidar = 0;
GaspardD 19:771bf61be276 149 }
GaspardD 21:de7a0a47f8a3 150 #if DEBUG > 0
GaspardD 19:771bf61be276 151 pc.printf("[INIT SAMPLE DONE]\r\n");
GaspardD 21:de7a0a47f8a3 152 #endif
GaspardD 19:771bf61be276 153 return;
GaspardD 19:771bf61be276 154 }
GaspardD 19:771bf61be276 155 void sampleLog(void)
GaspardD 19:771bf61be276 156 {
GaspardD 23:04d393220daa 157 #ifdef DUMP_SAMPLIG_PERIOD
GaspardD 23:04d393220daa 158 if(timerLog.read_us() > DUMP_SAMPLIG_PERIOD) {
GaspardD 22:26fc6e6f7a55 159 timerLog.reset();
GaspardD 22:26fc6e6f7a55 160 timerLog.start();
GaspardD 23:04d393220daa 161 #endif
GaspardD 22:26fc6e6f7a55 162 if(indexSample < TAILLE_SAMPLES) {
GaspardD 19:771bf61be276 163 #ifdef DLVV
GaspardD 22:26fc6e6f7a55 164 history[indexSample].states.murs_dlvv = (char)st_obstacle;
GaspardD 19:771bf61be276 165 #else
GaspardD 22:26fc6e6f7a55 166 history[indexSample].states.murs_dlvv = (char)st_murs;
GaspardD 19:771bf61be276 167 #endif
GaspardD 22:26fc6e6f7a55 168 history[indexSample].states.section = (char)st_currentSection;
GaspardD 22:26fc6e6f7a55 169 history[indexSample].states.maxSpeed = (char)st_maxSpeed;
GaspardD 22:26fc6e6f7a55 170 history[indexSample].states.throttle = (char)st_thro;
GaspardD 22:26fc6e6f7a55 171 history[indexSample].time = timeSinceStart.read_us() ;
GaspardD 29:fc984fe08ca7 172 history[indexSample].position45 = positionSurPiste45;
GaspardD 29:fc984fe08ca7 173 history[indexSample].position90 = positionSurPiste90;
ohlimi2 36:bccddd02966a 174 history[indexSample].largeurPiste = largeurPiste90;
GaspardD 22:26fc6e6f7a55 175 history[indexSample].pwm_thro_us = pulseSpeed_us;
GaspardD 22:26fc6e6f7a55 176 history[indexSample].pwm_dir_us = pulseDirection_us;
GaspardD 22:26fc6e6f7a55 177 history[indexSample].dist = tachySectionDist_cm,
GaspardD 41:4dd36f607279 178 history[indexSample].distLidar = distLidar;
GaspardD 23:04d393220daa 179 history[indexSample].strLidar = strengthLidar;
GaspardD 23:04d393220daa 180 indexSample++;
GaspardD 19:771bf61be276 181 #if DEBUG > 0
ohlimi2 36:bccddd02966a 182 pc.printf("\r\nodo:%d dist = %.4lf \tstrength = %.4ld \tC45D: %.4lf C45G: %.4lf C90D: %.4lf C90G: %.4lf looptime: %.4lf micros",tachySectionDist_cm,distLidar,strengthLidar,distMurD45Moy,distMurG45Moy,distMurD90Moy,distMurG90Moy,timeSinceStart.read_us());// output signal strength value
ohlimi2 36:bccddd02966a 183 pc.printf("\r\nstate Murs: %.4lf, state Section %.4lf, state MaxSpeed %.4lf, state Throttle %.4lf\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro);
ohlimi2 34:c98fd59c690a 184 //wait(2);
GaspardD 22:26fc6e6f7a55 185 timeSinceStart.reset();
GaspardD 22:26fc6e6f7a55 186 timeSinceStart.start();
GaspardD 19:771bf61be276 187 #endif
GaspardD 22:26fc6e6f7a55 188 }
GaspardD 22:26fc6e6f7a55 189 return;
GaspardD 23:04d393220daa 190 #ifdef DUMP_SAMPLIG_PERIOD
GaspardD 19:771bf61be276 191 }
GaspardD 23:04d393220daa 192 #endif
GaspardD 19:771bf61be276 193 }
GaspardD 19:771bf61be276 194 void transmitData(void)
GaspardD 19:771bf61be276 195 {
GaspardD 21:de7a0a47f8a3 196 #if DEBUG > 0
GaspardD 19:771bf61be276 197 pc.printf("[START TO TRANSMIT]\r\n");
GaspardD 21:de7a0a47f8a3 198 #endif
GaspardD 39:de3638276b7e 199 serialLidar.printf("time,position 45,position 90,largeur piste,pwm_thro_us,pwm_dir_us,dist,murs/dlvv,section,maxSpeed,throttle\r\n");
GaspardD 19:771bf61be276 200 for(int p=0; p<indexSample; p++) {
GaspardD 39:de3638276b7e 201 serialLidar.printf("%d,%.5f,%.5f,%.5f,%d,%d,%.5f,%d,%d,%d,%d\r\n",
GaspardD 21:de7a0a47f8a3 202 history[p].time,
GaspardD 29:fc984fe08ca7 203 history[p].position45,
GaspardD 29:fc984fe08ca7 204 history[p].position90,
GaspardD 39:de3638276b7e 205 history[p].largeurPiste,
GaspardD 21:de7a0a47f8a3 206 history[p].pwm_thro_us,
GaspardD 21:de7a0a47f8a3 207 history[p].pwm_dir_us,
GaspardD 21:de7a0a47f8a3 208 history[p].dist,
GaspardD 21:de7a0a47f8a3 209 history[p].states.murs_dlvv,
GaspardD 21:de7a0a47f8a3 210 history[p].states.section,
GaspardD 21:de7a0a47f8a3 211 history[p].states.maxSpeed,
GaspardD 21:de7a0a47f8a3 212 history[p].states.throttle);
GaspardD 19:771bf61be276 213 }
GaspardD 19:771bf61be276 214 return;
GaspardD 19:771bf61be276 215 }
GaspardD 19:771bf61be276 216
GaspardD 19:771bf61be276 217 #endif
GaspardD 19:771bf61be276 218
GaspardD 19:771bf61be276 219 // +++++++++++++++++++++++++++++++++++++++++++ FUNCTION UTILS +++++++++++++++++++++++++++++++++++++++++++
GaspardD 19:771bf61be276 220
GaspardD 19:771bf61be276 221 #if DEBUG >= -1
GaspardD 19:771bf61be276 222 void pressed(void)
GaspardD 19:771bf61be276 223 {
GaspardD 21:de7a0a47f8a3 224 #if DEBUG > 0
GaspardD 19:771bf61be276 225 pc.printf("[BTN PRESSED]\r\n");
GaspardD 21:de7a0a47f8a3 226 #endif
GaspardD 44:69751c05a8f3 227 if(!dumped)
GaspardD 44:69751c05a8f3 228 {
GaspardD 19:771bf61be276 229 transmitData();
GaspardD 44:69751c05a8f3 230 dumped = true;
GaspardD 44:69751c05a8f3 231 }else{
GaspardD 44:69751c05a8f3 232 initSamples();
GaspardD 44:69751c05a8f3 233 p_sectionCourante = &p_section1;
GaspardD 44:69751c05a8f3 234 dumped = false;
GaspardD 44:69751c05a8f3 235 }
GaspardD 19:771bf61be276 236 }
GaspardD 19:771bf61be276 237 #endif
GaspardD 19:771bf61be276 238
GaspardD 29:fc984fe08ca7 239 void initIntegrationTable()
GaspardD 29:fc984fe08ca7 240 {
GaspardD 41:4dd36f607279 241 for(int h=0; h<NB_INTEGRAL_SAMPLES; h++) {
GaspardD 29:fc984fe08ca7 242 lastDifferences90[h] = 0;
GaspardD 29:fc984fe08ca7 243 }
GaspardD 29:fc984fe08ca7 244 return;
GaspardD 29:fc984fe08ca7 245 }
GaspardD 29:fc984fe08ca7 246
GaspardD 21:de7a0a47f8a3 247
GaspardD 29:fc984fe08ca7 248 double getShortDistMoy(AnalogIn* p,double* tab,int size)
GaspardD 11:bc24b3ba51a9 249 {
GaspardD 29:fc984fe08ca7 250 tab[index_fifo_ir] = 3.3 * (double)p->read(); // on convertit directement en volts
GaspardD 29:fc984fe08ca7 251 //tension proportionelle à l'inverse de la distance en dessous de 2V
GaspardD 41:4dd36f607279 252 if(tab[index_fifo_ir]< 0.1034) {
GaspardD 41:4dd36f607279 253 tab[index_fifo_ir]= 0.11;
GaspardD 41:4dd36f607279 254 }
GaspardD 39:de3638276b7e 255 tab[index_fifo_ir] = (11.3531/(tab[index_fifo_ir]-0.1034))-0.42;
GaspardD 29:fc984fe08ca7 256
GaspardD 29:fc984fe08ca7 257 int sumMoy = 0;
GaspardD 29:fc984fe08ca7 258 for(int k=0; k<size; k++) {
GaspardD 29:fc984fe08ca7 259 sumMoy+=tab[k];
GaspardD 29:fc984fe08ca7 260 }
GaspardD 29:fc984fe08ca7 261 return sumMoy/size;
GaspardD 29:fc984fe08ca7 262 }
GaspardD 29:fc984fe08ca7 263
GaspardD 29:fc984fe08ca7 264 double getDistMoy(AnalogIn* p,double* tab,int size)
GaspardD 29:fc984fe08ca7 265 {
ohlimi2 47:b17061738568 266 double distcapteur = 0;
ohlimi2 51:09ecba68d0cf 267 distcapteur = 1.0/(0.0161 * (double)p->read()); // on convertit directement en volts
ohlimi2 51:09ecba68d0cf 268 //distcapteur = 1.0/(0.02 * (double)p->read()); // on convertit directement en volts
GaspardD 29:fc984fe08ca7 269 //tension proportionelle à l'inverse de la distance
ohlimi2 47:b17061738568 270 //tab[index_fifo_ir] = 1.0/(0.0161 * (double)tab[index_fifo_ir]);
ohlimi2 47:b17061738568 271 //int sumMoy = 0;
ohlimi2 47:b17061738568 272 //for(int k=0; k<size; k++) {
ohlimi2 47:b17061738568 273 // sumMoy+=tab[k];
ohlimi2 47:b17061738568 274 //}
ohlimi2 47:b17061738568 275 return distcapteur;
GaspardD 11:bc24b3ba51a9 276 }
GaspardD 39:de3638276b7e 277 void tachyCheck()
GaspardD 39:de3638276b7e 278 {
GaspardD 41:4dd36f607279 279 if(timerSinceTachy.read_us() > 500000) {
GaspardD 39:de3638276b7e 280 tachySpeed_cmps = 0.0;
GaspardD 39:de3638276b7e 281 }
GaspardD 41:4dd36f607279 282 return;
GaspardD 39:de3638276b7e 283 }
GaspardD 39:de3638276b7e 284
GaspardD 2:fd0ffe46a87d 285
GaspardD 12:51b1b40cc017 286 void it4cm()
GaspardD 12:51b1b40cc017 287 {
GaspardD 39:de3638276b7e 288 //le timer sert de flag sur le freinage:
GaspardD 39:de3638276b7e 289 //si le biniou ne bouge plus, il n'essaye plus de freiner tant qu'il n'y a pas un nouveau calcul de la vitesse
GaspardD 39:de3638276b7e 290 //dans la fonction tachyCheck() >> quand il n'y a pas eu d'IT en 500 ms si la vitesse est inférieure à 0.16 m/s, elle sera considérée comme nulle.
GaspardD 39:de3638276b7e 291
ohlimi2 36:bccddd02966a 292 tachySectionDist_cm += TACHY_CM;
ohlimi2 36:bccddd02966a 293 tachyTotalDist_cm += TACHY_CM;
GaspardD 39:de3638276b7e 294 tachySpeed_cmps = (TACHY_CM * 1000000.0)/(double)timerSinceTachy.read_us(); // a chaque IT on a parcouru 8 cm soit (8*1000000)/durée
ohlimi2 36:bccddd02966a 295 #if DEBUG > 2
ohlimi2 36:bccddd02966a 296 pc.printf("IT: distance parcourue %.4lf , vitesse:%.4lf \r\n",tachyTotalDist_cm,tachySpeed_cmps);
ohlimi2 36:bccddd02966a 297 #endif
GaspardD 39:de3638276b7e 298 timerSinceTachy.reset();
GaspardD 39:de3638276b7e 299 timerSinceTachy.start();
ohlimi2 36:bccddd02966a 300 return;
ohlimi2 36:bccddd02966a 301
GaspardD 19:771bf61be276 302 #if DEBUG > 0
GaspardD 14:d471faa7d1a2 303 pc.printf("IT tachy\r\n");
GaspardD 14:d471faa7d1a2 304 #endif
GaspardD 12:51b1b40cc017 305 }
GaspardD 12:51b1b40cc017 306
GaspardD 12:51b1b40cc017 307 void it_serial()
GaspardD 12:51b1b40cc017 308 {
GaspardD 12:51b1b40cc017 309 if(serialLidar.getc()==HEADER) { // determine data package frame header 0x59
GaspardD 12:51b1b40cc017 310 uart[0]=HEADER;
GaspardD 12:51b1b40cc017 311 if(serialLidar.getc()==HEADER) { //determine data package frame header 0x59
GaspardD 12:51b1b40cc017 312 uart[1]=HEADER;
GaspardD 12:51b1b40cc017 313 for(i=2; i<9; i++) { // store data to array
GaspardD 12:51b1b40cc017 314 uart[i]=serialLidar.getc();
GaspardD 12:51b1b40cc017 315 }
GaspardD 12:51b1b40cc017 316 check=uart[0]+uart[1]+uart[2]+uart[3]+uart[4]+uart[5]+uart[6]+uart[7];
GaspardD 12:51b1b40cc017 317 if(uart[8]==(check&0xff)) { // check the received data as per protocols
GaspardD 23:04d393220daa 318 distLidarPrev = distLidar;
GaspardD 12:51b1b40cc017 319 distLidar=uart[2]+uart[3]*256;// calculate distance value
GaspardD 23:04d393220daa 320 strengthLidarPrev = strengthLidar;
GaspardD 12:51b1b40cc017 321 strengthLidar=uart[4]+uart[5]*256;// calculate signal strength value
GaspardD 42:3f12252862b9 322 timersinceSerial.reset();
GaspardD 42:3f12252862b9 323 timersinceSerial.start();
GaspardD 12:51b1b40cc017 324 }
GaspardD 12:51b1b40cc017 325 }
GaspardD 12:51b1b40cc017 326 }
GaspardD 12:51b1b40cc017 327 }
GaspardD 19:771bf61be276 328 // +++++++++++++++++++++++++++++++++++++++++++ STATES MACHINES +++++++++++++++++++++++++++++++++++++++++++
GaspardD 19:771bf61be276 329
GaspardD 8:1d8c3ca5e508 330 //########## INIT STATES MACHINES ##########
GaspardD 11:bc24b3ba51a9 331 void mursInit(void)
GaspardD 8:1d8c3ca5e508 332 {
GaspardD 39:de3638276b7e 333 #if DEBUG > 0
GaspardD 13:af9a59ccf60b 334 pc.printf("Init Murs\r\n");
GaspardD 13:af9a59ccf60b 335 #endif
GaspardD 13:af9a59ccf60b 336 timeSinceStart.start();
GaspardD 29:fc984fe08ca7 337 st_murs=REF_BIDIR;
GaspardD 19:771bf61be276 338 PwmDirection.period_us(SPEED_PERIOD_US);
GaspardD 17:8c465656eea4 339 PwmDirection.pulsewidth_us(DIRECTION_PULSE_MIDDLE);// milieu
GaspardD 8:1d8c3ca5e508 340 return;
GaspardD 8:1d8c3ca5e508 341 }
GaspardD 11:bc24b3ba51a9 342 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 343 void obstacleInit(void)
GaspardD 8:1d8c3ca5e508 344 {
GaspardD 19:771bf61be276 345 #if DEBUG > 0
GaspardD 13:af9a59ccf60b 346 pc.printf("Init Obstacle\r\n");
GaspardD 13:af9a59ccf60b 347 #endif
GaspardD 29:fc984fe08ca7 348 st_speedLimit=ALL_CLEAR;
GaspardD 8:1d8c3ca5e508 349 return;
GaspardD 8:1d8c3ca5e508 350 }
GaspardD 11:bc24b3ba51a9 351 #endif
GaspardD 11:bc24b3ba51a9 352 void sectionInit(void)
GaspardD 2:fd0ffe46a87d 353 {
GaspardD 19:771bf61be276 354 #if DEBUG > 0
GaspardD 13:af9a59ccf60b 355 pc.printf("Init Section\r\n");
GaspardD 13:af9a59ccf60b 356 #endif
GaspardD 11:bc24b3ba51a9 357 st_currentSection=ARRET;
GaspardD 14:d471faa7d1a2 358 p_sectionCourante=&p_section1;
GaspardD 12:51b1b40cc017 359 it_tachymeter.fall(&it4cm);
GaspardD 39:de3638276b7e 360 timerSinceTachy.start();
GaspardD 39:de3638276b7e 361 tachySectionDist_cm = 0;
GaspardD 39:de3638276b7e 362 tachyStepsRegister = 0;
GaspardD 11:bc24b3ba51a9 363
GaspardD 23:04d393220daa 364 //section de test 1
ohlimi2 52:228703200e35 365 p_section1.nextSection =&p_section2;// &p_section2;
GaspardD 29:fc984fe08ca7 366 p_section1.consigne_position = 75.0;
ohlimi2 52:228703200e35 367 p_section1.targetSpeed_cmps = 600.0;
GaspardD 29:fc984fe08ca7 368 p_section1.slowSpeed_cmps = 328.0;
GaspardD 21:de7a0a47f8a3 369 p_section1.coef_p_speed = 1;
GaspardD 29:fc984fe08ca7 370 p_section1.lidarWarningDist_cm = 120.0;
ohlimi2 52:228703200e35 371 p_section1.lng_section_cm = 1300.0;//13m
ohlimi2 52:228703200e35 372 // coef P
ohlimi2 52:228703200e35 373 p_section1.coef_pd90 = -0.03;
ohlimi2 52:228703200e35 374 p_section1.coef_pg90 = 0.03;
ohlimi2 52:228703200e35 375 p_section1.coef_pd45 = -0.03;
ohlimi2 52:228703200e35 376 p_section1.coef_pg45 = 0.03;
ohlimi2 52:228703200e35 377 // le reste
ohlimi2 52:228703200e35 378 p_section1.coef_dd90 = 0.0;
ohlimi2 52:228703200e35 379 p_section1.coef_dg90 = 0.0;
ohlimi2 52:228703200e35 380 p_section1.coef_dd45 = 0.0;
ohlimi2 52:228703200e35 381 p_section1.coef_dg45 = 0.0;
ohlimi2 52:228703200e35 382
ohlimi2 51:09ecba68d0cf 383 p_section1.coef_i90 = 0.0;
ohlimi2 51:09ecba68d0cf 384 p_section1.coef_i45 = 0.0;
GaspardD 39:de3638276b7e 385
GaspardD 39:de3638276b7e 386 //section de test
ohlimi2 46:9262b07b0833 387 p_section2.nextSection = &p_section3;
GaspardD 39:de3638276b7e 388 p_section2.consigne_position = 75.0;
ohlimi2 52:228703200e35 389 p_section2.targetSpeed_cmps = 400.0;
GaspardD 39:de3638276b7e 390 p_section2.slowSpeed_cmps = 328.0;
GaspardD 39:de3638276b7e 391 p_section2.coef_p_speed = 1.0;
GaspardD 39:de3638276b7e 392 p_section2.lidarWarningDist_cm = 300.0;
ohlimi2 52:228703200e35 393 p_section2.lng_section_cm = 600.0;//2m
ohlimi2 52:228703200e35 394 // coef en P
ohlimi2 52:228703200e35 395 p_section2.coef_pd90 = -0.03;
ohlimi2 52:228703200e35 396 p_section2.coef_pg90 = 0.03;
ohlimi2 52:228703200e35 397 p_section2.coef_pd45 = -0.03;
ohlimi2 52:228703200e35 398 p_section2.coef_pg45 = 0.03;
ohlimi2 52:228703200e35 399 // le reste
ohlimi2 52:228703200e35 400 p_section2.coef_dd90 = 0.0;
ohlimi2 52:228703200e35 401 p_section2.coef_dg90 = 0.0;
ohlimi2 52:228703200e35 402 p_section2.coef_dd45 = 0.0;
ohlimi2 52:228703200e35 403 p_section2.coef_dg45 = 0.0;
ohlimi2 52:228703200e35 404
ohlimi2 51:09ecba68d0cf 405 p_section2.coef_i90 = 0.0;
ohlimi2 51:09ecba68d0cf 406 p_section2.coef_i45 = 0.0;
ohlimi2 52:228703200e35 407
GaspardD 23:04d393220daa 408
ohlimi2 46:9262b07b0833 409 p_section3.nextSection = NULL;
ohlimi2 46:9262b07b0833 410 p_section3.consigne_position = 75.0;
ohlimi2 46:9262b07b0833 411 p_section3.targetSpeed_cmps = 0.0;
ohlimi2 46:9262b07b0833 412 p_section3.slowSpeed_cmps = 0.0;
ohlimi2 46:9262b07b0833 413 p_section3.coef_p_speed = 1.0;
ohlimi2 46:9262b07b0833 414 p_section3.lidarWarningDist_cm = 300.0;
ohlimi2 52:228703200e35 415 p_section3.lng_section_cm = 100.0;//2m
ohlimi2 52:228703200e35 416 // coef en P
ohlimi2 52:228703200e35 417 p_section3.coef_pd90 = -0.03;
ohlimi2 52:228703200e35 418 p_section3.coef_pg90 = 0.03;
ohlimi2 52:228703200e35 419 p_section3.coef_pd45 = -0.03;
ohlimi2 52:228703200e35 420 p_section3.coef_pg45 = 0.03;
ohlimi2 52:228703200e35 421
ohlimi2 52:228703200e35 422 p_section3.coef_dd90 = 0.0;
ohlimi2 52:228703200e35 423 p_section3.coef_dg90 = 0.0;
ohlimi2 52:228703200e35 424 p_section3.coef_dd45 = 0.0;
ohlimi2 52:228703200e35 425 p_section3.coef_dg45 = 0.0;
ohlimi2 52:228703200e35 426
ohlimi2 52:228703200e35 427 p_section3.coef_i45 = 0.0;
ohlimi2 51:09ecba68d0cf 428 p_section3.coef_i90 = 0.0;
ohlimi2 46:9262b07b0833 429
GaspardD 8:1d8c3ca5e508 430 return;
GaspardD 8:1d8c3ca5e508 431 }
GaspardD 11:bc24b3ba51a9 432
GaspardD 11:bc24b3ba51a9 433 void maxSpeedInit(void)
GaspardD 11:bc24b3ba51a9 434 {
GaspardD 19:771bf61be276 435 #if DEBUG > 0
GaspardD 13:af9a59ccf60b 436 pc.printf("Init Max Speed\r\n");
GaspardD 13:af9a59ccf60b 437 #endif
GaspardD 11:bc24b3ba51a9 438 st_maxSpeed=SPEED_MAX;
GaspardD 11:bc24b3ba51a9 439 maxSpeed_cmps= p_sectionCourante->targetSpeed_cmps;
GaspardD 11:bc24b3ba51a9 440 serialLidar.baud(115200);
ohlimi2 52:228703200e35 441 serialDebug.baud(115200);
GaspardD 14:d471faa7d1a2 442 serialLidar.attach(&it_serial);
GaspardD 11:bc24b3ba51a9 443 return;
GaspardD 11:bc24b3ba51a9 444 }
GaspardD 11:bc24b3ba51a9 445
GaspardD 11:bc24b3ba51a9 446 void throttleInit(void)
GaspardD 8:1d8c3ca5e508 447 {
GaspardD 19:771bf61be276 448 #if DEBUG > 0
GaspardD 13:af9a59ccf60b 449 pc.printf("Init Throttle\r\n");
GaspardD 13:af9a59ccf60b 450 #endif
GaspardD 21:de7a0a47f8a3 451 st_thro = REGULATION_SPEED;
ohlimi2 36:bccddd02966a 452 PwmMotor.period_us(DIRECTION_PERIOD_MS); //20 ms is default
GaspardD 19:771bf61be276 453 PwmMotor.pulsewidth_us(1000);//MIN
GaspardD 12:51b1b40cc017 454 wait(3);
GaspardD 19:771bf61be276 455 PwmMotor.pulsewidth_us(2000);//MAX
GaspardD 12:51b1b40cc017 456 wait(1);
GaspardD 19:771bf61be276 457 PwmMotor.pulsewidth_us(1500);//ZEROING
GaspardD 12:51b1b40cc017 458 wait(1);
GaspardD 19:771bf61be276 459 pulseSpeed_us = INITAL_PULSE_SPEED_US;
GaspardD 19:771bf61be276 460 #if DEBUG > 0
ohlimi2 36:bccddd02966a 461 pc.printf("temps init: %.4lf micros\r\n",timeSinceStart.read_us());
ohlimi2 36:bccddd02966a 462 pc.printf("\r\nStates INIT: state Murs: %.4lf, state Section %.4lf, state MaxSpeed %.4lf, state Throttle %.4lf\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro);
GaspardD 13:af9a59ccf60b 463 timeSinceStart.reset();
GaspardD 13:af9a59ccf60b 464 timeSinceStart.start();
GaspardD 13:af9a59ccf60b 465 #endif
GaspardD 23:04d393220daa 466 #ifdef DUMP_SAMPLIG_PERIOD
GaspardD 22:26fc6e6f7a55 467 timerLog.start();
GaspardD 22:26fc6e6f7a55 468 #endif
GaspardD 22:26fc6e6f7a55 469
GaspardD 8:1d8c3ca5e508 470 return;
GaspardD 8:1d8c3ca5e508 471 }
GaspardD 2:fd0ffe46a87d 472
GaspardD 8:1d8c3ca5e508 473
GaspardD 8:1d8c3ca5e508 474 //########## UPDATE STATES ##########
GaspardD 11:bc24b3ba51a9 475 void mursUpdate(void)
GaspardD 2:fd0ffe46a87d 476 {
GaspardD 14:d471faa7d1a2 477 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 478 pc.printf("\r\nUpdate MURS\r\n");
GaspardD 14:d471faa7d1a2 479 #endif
GaspardD 11:bc24b3ba51a9 480 //lectures
GaspardD 29:fc984fe08ca7 481
GaspardD 29:fc984fe08ca7 482 distMurG45Moy = getDistMoy(&anaG45,distMurG45,NB_ECHANTILLONS_IR);
GaspardD 29:fc984fe08ca7 483 distMurD45Moy = getDistMoy(&anaD45,distMurD45,NB_ECHANTILLONS_IR);
GaspardD 29:fc984fe08ca7 484 distMurG90Moy = getDistMoy(&anaG90,distMurG90,NB_ECHANTILLONS_IR);
ohlimi2 36:bccddd02966a 485 distMurD90Moy = getDistMoy(&anaD90,distMurD90,NB_ECHANTILLONS_IR);
ohlimi2 52:228703200e35 486 //shortDistMurG90Moy = getShortDistMoy(&anaShortG90,shortDistMurD90,NB_ECHANTILLONS_IR);
ohlimi2 52:228703200e35 487 //shortDistMurD90Moy = getShortDistMoy(&anaShortD90,shortDistMurD90,NB_ECHANTILLONS_IR);
GaspardD 11:bc24b3ba51a9 488 index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR;
GaspardD 29:fc984fe08ca7 489
ohlimi2 36:bccddd02966a 490
GaspardD 41:4dd36f607279 491 trueDistMurD90Moy = distMurD90Moy;
GaspardD 41:4dd36f607279 492 trueDistMurG90Moy = distMurG90Moy;
ohlimi2 52:228703200e35 493 /*
GaspardD 41:4dd36f607279 494 if(shortDistMurG90Moy < DIST_MIN_LONG_CM) {
GaspardD 29:fc984fe08ca7 495 trueDistMurG90Moy = shortDistMurG90Moy;
GaspardD 41:4dd36f607279 496 } else {
GaspardD 29:fc984fe08ca7 497 trueDistMurG90Moy = distMurG90Moy;
GaspardD 29:fc984fe08ca7 498 }
GaspardD 41:4dd36f607279 499 if(shortDistMurD90Moy < DIST_MIN_LONG_CM) {
GaspardD 29:fc984fe08ca7 500 trueDistMurD90Moy = shortDistMurD90Moy;
GaspardD 41:4dd36f607279 501 } else {
GaspardD 29:fc984fe08ca7 502 trueDistMurD90Moy = distMurD90Moy;
GaspardD 39:de3638276b7e 503 }
ohlimi2 52:228703200e35 504 */
GaspardD 29:fc984fe08ca7 505
GaspardD 29:fc984fe08ca7 506 #ifdef DLVV
GaspardD 29:fc984fe08ca7 507 switch (st_obstacle) {
GaspardD 41:4dd36f607279 508 case FRONT_OBSTRUCTED:
GaspardD 41:4dd36f607279 509 st_tmpMurs = REF_A_GAUCHE;
GaspardD 41:4dd36f607279 510 return;
GaspardD 41:4dd36f607279 511 case RIGHT_OBSTRUCTED:
GaspardD 41:4dd36f607279 512 st_tmpMurs = REF_A_GAUCHE;
GaspardD 41:4dd36f607279 513 return;
GaspardD 41:4dd36f607279 514 case LEFT_OBSTRUCTED:
GaspardD 41:4dd36f607279 515 st_tmpMurs = REF_A_DROITE;
GaspardD 41:4dd36f607279 516 return;
GaspardD 41:4dd36f607279 517 default:
GaspardD 41:4dd36f607279 518 break;
GaspardD 29:fc984fe08ca7 519 }
GaspardD 29:fc984fe08ca7 520
GaspardD 29:fc984fe08ca7 521 #endif
GaspardD 41:4dd36f607279 522 st_tmpMurs = REF_BIDIR;
GaspardD 29:fc984fe08ca7 523
GaspardD 41:4dd36f607279 524 st_murs = st_tmpMurs;
GaspardD 12:51b1b40cc017 525
GaspardD 8:1d8c3ca5e508 526 return;
GaspardD 8:1d8c3ca5e508 527 }
GaspardD 11:bc24b3ba51a9 528 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 529 void obstacleUpdate(void)
GaspardD 8:1d8c3ca5e508 530 {
GaspardD 8:1d8c3ca5e508 531 return;
GaspardD 8:1d8c3ca5e508 532 }
GaspardD 11:bc24b3ba51a9 533 #endif
GaspardD 11:bc24b3ba51a9 534
GaspardD 11:bc24b3ba51a9 535 void sectionUpdate(void)
GaspardD 8:1d8c3ca5e508 536 {
GaspardD 14:d471faa7d1a2 537 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 538 pc.printf("\r\nUpdate Section\r\n");
GaspardD 14:d471faa7d1a2 539 #endif
GaspardD 43:d2164ee3336a 540
GaspardD 43:d2164ee3336a 541 if(p_sectionCourante == NULL) {
GaspardD 43:d2164ee3336a 542 st_tmpSection = ARRET;
GaspardD 43:d2164ee3336a 543 } else {
GaspardD 43:d2164ee3336a 544
GaspardD 43:d2164ee3336a 545 switch (st_currentSection) {
GaspardD 43:d2164ee3336a 546 case RUNNING_SECTION:
GaspardD 43:d2164ee3336a 547 if(tachySectionDist_cm > p_sectionCourante->lng_section_cm) { //on pourrait rajouter un test lidar
GaspardD 43:d2164ee3336a 548 st_tmpSection = LOADING_SECTION;
GaspardD 43:d2164ee3336a 549 } else {
GaspardD 43:d2164ee3336a 550 return;
GaspardD 43:d2164ee3336a 551 }
GaspardD 43:d2164ee3336a 552 break;
GaspardD 43:d2164ee3336a 553 case LOADING_SECTION:
GaspardD 43:d2164ee3336a 554 if(p_sectionCourante != NULL) { //la section a ete chargee dans sectionOutput
GaspardD 43:d2164ee3336a 555 st_tmpSection = RUNNING_SECTION;
GaspardD 43:d2164ee3336a 556 } else {
GaspardD 43:d2164ee3336a 557 st_tmpSection=ARRET;
GaspardD 43:d2164ee3336a 558 }
GaspardD 43:d2164ee3336a 559 break;
GaspardD 43:d2164ee3336a 560 case ARRET:
GaspardD 43:d2164ee3336a 561 if(p_sectionCourante != NULL) {
GaspardD 43:d2164ee3336a 562 st_tmpSection = RUNNING_SECTION;
GaspardD 43:d2164ee3336a 563 } else {
GaspardD 43:d2164ee3336a 564 return;
GaspardD 43:d2164ee3336a 565 }
GaspardD 43:d2164ee3336a 566 break;
GaspardD 43:d2164ee3336a 567 default:
GaspardD 43:d2164ee3336a 568 break;
GaspardD 43:d2164ee3336a 569 }
GaspardD 11:bc24b3ba51a9 570 }
GaspardD 11:bc24b3ba51a9 571 st_currentSection = st_tmpSection;
GaspardD 8:1d8c3ca5e508 572 return;
GaspardD 8:1d8c3ca5e508 573 }
GaspardD 8:1d8c3ca5e508 574
GaspardD 11:bc24b3ba51a9 575 void maxSpeedUpdate(void)
GaspardD 8:1d8c3ca5e508 576 {
GaspardD 14:d471faa7d1a2 577 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 578 pc.printf("\r\nUpdate MaxSpeed\r\n");
GaspardD 14:d471faa7d1a2 579 #endif
GaspardD 45:7d67809bd7bf 580 if(p_sectionCourante == NULL) {
GaspardD 43:d2164ee3336a 581 st_tmpMaxSpeed = BLOCKED;
GaspardD 43:d2164ee3336a 582 } else if(st_maxSpeed == BLOCKED && strengthLidar > LIDAR_STRENGTH_THRESOLD && distLidar < p_sectionCourante->lidarWarningDist_cm) {
GaspardD 41:4dd36f607279 583 st_tmpMaxSpeed = BLOCKED;
GaspardD 41:4dd36f607279 584 } else if( strengthLidar > LIDAR_STRENGTH_THRESOLD ) {
GaspardD 23:04d393220daa 585 if( distLidar > p_sectionCourante->lidarWarningDist_cm ) {
GaspardD 19:771bf61be276 586 st_tmpMaxSpeed = SPEED_MAX;
GaspardD 29:fc984fe08ca7 587 } else if( strengthLidar > LIDAR_STRENGTH_THRESOLD &&
GaspardD 24:698fefbbee00 588 strengthLidarPrev > LIDAR_STRENGTH_THRESOLD &&
GaspardD 24:698fefbbee00 589 distLidar < p_sectionCourante->lidarWarningDist_cm &&
GaspardD 41:4dd36f607279 590 distLidarPrev-distLidar > 3) {
GaspardD 21:de7a0a47f8a3 591 st_tmpMaxSpeed = BLOCKED;
GaspardD 41:4dd36f607279 592 } else {
GaspardD 24:698fefbbee00 593 st_tmpMaxSpeed = SPEED_WARNING;
GaspardD 24:698fefbbee00 594 }
GaspardD 21:de7a0a47f8a3 595 } else {
GaspardD 11:bc24b3ba51a9 596 st_tmpMaxSpeed = SPEED_MAX;
GaspardD 11:bc24b3ba51a9 597 }
GaspardD 11:bc24b3ba51a9 598
GaspardD 11:bc24b3ba51a9 599 st_maxSpeed = st_tmpMaxSpeed;
GaspardD 11:bc24b3ba51a9 600 return;
GaspardD 11:bc24b3ba51a9 601 }
GaspardD 11:bc24b3ba51a9 602
GaspardD 11:bc24b3ba51a9 603 void throttleUpdate(void)
GaspardD 11:bc24b3ba51a9 604 {
GaspardD 14:d471faa7d1a2 605 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 606 pc.printf("\r\nUpdate Throttle\r\n");
GaspardD 14:d471faa7d1a2 607 #endif
GaspardD 11:bc24b3ba51a9 608 switch (st_thro) {
GaspardD 21:de7a0a47f8a3 609 case REGULATION_SPEED:
GaspardD 24:698fefbbee00 610 if( st_currentSection == ARRET ||
GaspardD 41:4dd36f607279 611 st_maxSpeed == BLOCKED ) {
GaspardD 19:771bf61be276 612 st_tmpThro = BRAKING;
GaspardD 29:fc984fe08ca7 613 }
GaspardD 27:f8c3f1524a64 614 #ifdef FREINAGE_ADAPTATIF
GaspardD 41:4dd36f607279 615 else if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS)) {
GaspardD 24:698fefbbee00 616 st_tmpThro = BRAKING;
GaspardD 29:fc984fe08ca7 617 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
GaspardD 24:698fefbbee00 618 brakingTimer.reset();
GaspardD 24:698fefbbee00 619 brakingTimer.start();
GaspardD 27:f8c3f1524a64 620 }
GaspardD 27:f8c3f1524a64 621 #endif
GaspardD 41:4dd36f607279 622 else {
GaspardD 19:771bf61be276 623 return;
GaspardD 19:771bf61be276 624 }
GaspardD 19:771bf61be276 625 break;
GaspardD 16:63690703b5b6 626 case BRAKING:
GaspardD 42:3f12252862b9 627 if(st_maxSpeed == BLOCKED && timersinceSerial.read_us() > 200000) {
GaspardD 42:3f12252862b9 628 p_sectionCourante= NULL;
GaspardD 42:3f12252862b9 629 st_tmpThro = STOPPED;
GaspardD 42:3f12252862b9 630 } else if( st_maxSpeed == BLOCKED
GaspardD 41:4dd36f607279 631 #ifdef FREINAGE_ADAPTATIF
GaspardD 42:3f12252862b9 632 || brakingTimer.read_us() < brakingDurationNeeded_us
GaspardD 41:4dd36f607279 633 #endif
GaspardD 42:3f12252862b9 634 ) {
GaspardD 23:04d393220daa 635 st_tmpThro = BRAKING;
GaspardD 41:4dd36f607279 636 } else if(st_currentSection == ARRET) {
GaspardD 21:de7a0a47f8a3 637 st_tmpThro = STOPPED;
GaspardD 41:4dd36f607279 638 } else {
GaspardD 41:4dd36f607279 639 #ifdef FREINAGE_ADAPTATIF
GaspardD 24:698fefbbee00 640 brakingDurationNeeded_us = 0;
GaspardD 41:4dd36f607279 641 #endif
GaspardD 21:de7a0a47f8a3 642 st_tmpThro = REGULATION_SPEED;
GaspardD 21:de7a0a47f8a3 643 }
GaspardD 19:771bf61be276 644 break;
GaspardD 19:771bf61be276 645 case STOPPED:
GaspardD 19:771bf61be276 646 if(st_currentSection == RUNNING_SECTION) {
GaspardD 22:26fc6e6f7a55 647 st_tmpThro = REGULATION_SPEED;
GaspardD 19:771bf61be276 648 } else {
GaspardD 19:771bf61be276 649 st_tmpThro = STOPPED;
GaspardD 16:63690703b5b6 650 }
GaspardD 19:771bf61be276 651 break;
GaspardD 19:771bf61be276 652 default:
GaspardD 19:771bf61be276 653 break;
GaspardD 11:bc24b3ba51a9 654 }
GaspardD 11:bc24b3ba51a9 655 st_thro = st_tmpThro;
GaspardD 8:1d8c3ca5e508 656 return;
GaspardD 8:1d8c3ca5e508 657 }
GaspardD 2:fd0ffe46a87d 658
GaspardD 8:1d8c3ca5e508 659 //########## OUTPUT STATES ##########
GaspardD 11:bc24b3ba51a9 660 //updating output parameters
GaspardD 11:bc24b3ba51a9 661 void mursOutput(void)
GaspardD 12:51b1b40cc017 662 {
GaspardD 43:d2164ee3336a 663 if(p_sectionCourante != NULL) {
GaspardD 43:d2164ee3336a 664 //pour dériver
GaspardD 43:d2164ee3336a 665 positionSurPiste45Prev = positionSurPiste45;
GaspardD 43:d2164ee3336a 666 positionSurPiste90Prev = positionSurPiste90;
ohlimi2 52:228703200e35 667
ohlimi2 52:228703200e35 668 // distMurG45MoyPrev = distMurG45Moy ;
ohlimi2 52:228703200e35 669 // distMurD45MoyPrev = distMurD45Moy ;
ohlimi2 52:228703200e35 670 // distMurD45MoyPrev = distMurD45Moy ;
ohlimi2 52:228703200e35 671 // distMurD45MoyPrev = distMurD45Moy ;
ohlimi2 52:228703200e35 672
ohlimi2 52:228703200e35 673
ohlimi2 52:228703200e35 674
ohlimi2 52:228703200e35 675
ohlimi2 52:228703200e35 676
ohlimi2 52:228703200e35 677
GaspardD 14:d471faa7d1a2 678 #if (DEBUG > 3)
GaspardD 43:d2164ee3336a 679 pc.printf("\r\n Output MURS\r\n");
GaspardD 14:d471faa7d1a2 680 #endif
GaspardD 23:04d393220daa 681
GaspardD 43:d2164ee3336a 682 switch (st_murs) {
GaspardD 43:d2164ee3336a 683 case REF_A_GAUCHE://par defaut, on compte à partir de la bordure gauche
GaspardD 43:d2164ee3336a 684 largeurPiste90 = 150.0;
GaspardD 43:d2164ee3336a 685 positionSurPiste90 = ( trueDistMurG90Moy + DEMI_LARGEUR_BINIOU_CM );
GaspardD 43:d2164ee3336a 686 positionSurPiste45 = ( distMurG45Moy * 1.414214/2.0 ) + DEMI_LARGEUR_BINIOU_CM;
GaspardD 39:de3638276b7e 687 #ifdef OMAR
GaspardD 43:d2164ee3336a 688 pc.printf("REF_A_GAUCHE\r\n");
GaspardD 39:de3638276b7e 689 #endif
GaspardD 43:d2164ee3336a 690 break;
GaspardD 43:d2164ee3336a 691 case REF_A_DROITE://par defaut, on compte à partir de la bordure gauche
GaspardD 43:d2164ee3336a 692 largeurPiste90 = 150.0;
GaspardD 43:d2164ee3336a 693 positionSurPiste90 = largeurPiste90 - ( DEMI_LARGEUR_BINIOU_CM + trueDistMurD90Moy ) ;
ohlimi2 47:b17061738568 694 positionSurPiste45 = largeurPiste90 - ( distMurD45Moy * (1.414214)/2.0 ) + DEMI_LARGEUR_BINIOU_CM;
GaspardD 39:de3638276b7e 695 #ifdef OMAR
GaspardD 43:d2164ee3336a 696 pc.printf("REF_A_DROITE\r\n");
GaspardD 39:de3638276b7e 697 #endif
GaspardD 43:d2164ee3336a 698 break;
GaspardD 43:d2164ee3336a 699 default://REF_BIDIR
GaspardD 43:d2164ee3336a 700 largeurPiste90 = trueDistMurG90Moy + trueDistMurD90Moy ;
GaspardD 43:d2164ee3336a 701 largeurPiste45 = distMurG45Moy + distMurD45Moy ;
GaspardD 43:d2164ee3336a 702 positionSurPiste90 = (trueDistMurG90Moy );
GaspardD 43:d2164ee3336a 703 positionSurPiste45 = (distMurG45Moy);
GaspardD 39:de3638276b7e 704 #ifdef OMAR
GaspardD 43:d2164ee3336a 705 pc.printf("REF_BIDIR\r\n");
GaspardD 39:de3638276b7e 706 #endif
GaspardD 43:d2164ee3336a 707 break;
GaspardD 43:d2164ee3336a 708 }
GaspardD 23:04d393220daa 709
GaspardD 22:26fc6e6f7a55 710
GaspardD 43:d2164ee3336a 711 //deriv correction
GaspardD 43:d2164ee3336a 712 derive45 = positionSurPiste45 - positionSurPiste45Prev;
GaspardD 43:d2164ee3336a 713 derive90 = positionSurPiste90 - positionSurPiste90Prev;
GaspardD 39:de3638276b7e 714
ohlimi2 52:228703200e35 715
ohlimi2 52:228703200e35 716
GaspardD 39:de3638276b7e 717
GaspardD 43:d2164ee3336a 718 //integral correction
GaspardD 43:d2164ee3336a 719 lastDifferences90[lastDifferenceIndex] = (p_sectionCourante->consigne_position - positionSurPiste90);
GaspardD 43:d2164ee3336a 720 integralSum=0;
GaspardD 43:d2164ee3336a 721 for(int f=0; f<NB_INTEGRAL_SAMPLES; f++) {
GaspardD 43:d2164ee3336a 722 integralSum+=lastDifferences90[f];
GaspardD 43:d2164ee3336a 723 }
GaspardD 43:d2164ee3336a 724 lastDifferenceIndex = (lastDifferenceIndex + 1)%NB_INTEGRAL_SAMPLES;
GaspardD 39:de3638276b7e 725
GaspardD 41:4dd36f607279 726
GaspardD 41:4dd36f607279 727
GaspardD 41:4dd36f607279 728
GaspardD 43:d2164ee3336a 729 //application des coefficients
GaspardD 41:4dd36f607279 730
GaspardD 43:d2164ee3336a 731 pulseDirection_us = (int)(
ohlimi2 52:228703200e35 732 ((distMurG45Moy / 0.27) * p_sectionCourante->coef_pg45)
ohlimi2 52:228703200e35 733 + ((distMurD45Moy / 0.27) * p_sectionCourante->coef_pd45)
ohlimi2 52:228703200e35 734 + ((distMurD90Moy / 0.17) * p_sectionCourante->coef_pg90)
ohlimi2 52:228703200e35 735 + ((distMurD90Moy / 0.17) * p_sectionCourante->coef_pd90)
ohlimi2 52:228703200e35 736
ohlimi2 52:228703200e35 737
ohlimi2 52:228703200e35 738 //+ ( -integralSum * p_sectionCourante->coef_i90)
ohlimi2 47:b17061738568 739 )//(300.0/(tachySpeed_cmps+1.0)) // faire du test reel pour afiné ou un calcul
GaspardD 43:d2164ee3336a 740 + DIRECTION_PULSE_MIDDLE;
ohlimi2 52:228703200e35 741
ohlimi2 52:228703200e35 742
ohlimi2 52:228703200e35 743
ohlimi2 52:228703200e35 744
ohlimi2 52:228703200e35 745
ohlimi2 52:228703200e35 746
ohlimi2 52:228703200e35 747
ohlimi2 52:228703200e35 748
ohlimi2 52:228703200e35 749
ohlimi2 52:228703200e35 750 /*
ohlimi2 52:228703200e35 751 serialDebug.printf("derive45 => %.4lf \r\n ",derive45);
ohlimi2 52:228703200e35 752 serialDebug.printf("derive90 => %.4lf \r\n ",derive90);
ohlimi2 52:228703200e35 753 serialDebug.printf("distMurG45Moy => %.4lf \r\n ",distMurG45Moy);
ohlimi2 52:228703200e35 754 serialDebug.printf("distMurD45Moy => %.4lf \r\n ",distMurD45Moy);
ohlimi2 52:228703200e35 755 serialDebug.printf("trueDistMurG90Moy => %.4lf \r\n ",trueDistMurG90Moy);
ohlimi2 52:228703200e35 756 serialDebug.printf("trueDistMurD90Moy => %.4lf \r\n ",trueDistMurD90Moy);
ohlimi2 52:228703200e35 757 serialDebug.printf("positionSurPiste90 => %.4lf \r\n ",positionSurPiste90);
ohlimi2 52:228703200e35 758 serialDebug.printf("positionSurPiste45 => %.4lf \r\n ",positionSurPiste45);
ohlimi2 52:228703200e35 759 serialDebug.printf("largeurPiste90 => %.4lf \r\n ",largeurPiste90);*/
GaspardD 41:4dd36f607279 760
GaspardD 41:4dd36f607279 761
GaspardD 39:de3638276b7e 762 #ifdef OMAR
GaspardD 43:d2164ee3336a 763 pc.printf("p_sectionCourante->consigne_position => %.4lf \r\n ",p_sectionCourante->consigne_position);
GaspardD 43:d2164ee3336a 764 pc.printf("positionSurPiste45 => %.4lf \r\n ",positionSurPiste45);
GaspardD 43:d2164ee3336a 765 pc.printf("p_sectionCourante->coef_p => %.4lf \r\n ",p_sectionCourante->coef_p);
GaspardD 39:de3638276b7e 766 #endif
GaspardD 21:de7a0a47f8a3 767 //gestioon du dépassement
GaspardD 43:d2164ee3336a 768 if(pulseDirection_us > DIRECTION_PULSE_MAX) { //POUR TOURNER A GAUCHE
GaspardD 19:771bf61be276 769 #if DEBUG > 1
GaspardD 43:d2164ee3336a 770 pc.printf("!!! OVER PWM Direction pulse: %.4lf\r\n",pulseDirection_us);
GaspardD 21:de7a0a47f8a3 771 #endif
GaspardD 43:d2164ee3336a 772 pulseDirection_us = DIRECTION_PULSE_MAX;
GaspardD 43:d2164ee3336a 773 } else if(pulseDirection_us < DIRECTION_PULSE_MIN ) { //POUR TOURNER A DROITE
GaspardD 21:de7a0a47f8a3 774 #if DEBUG > 1
GaspardD 43:d2164ee3336a 775 pc.printf("!!! UNDER PWM Direction pulse: %.4lf\r\n",pulseDirection_us);
GaspardD 19:771bf61be276 776 #endif
GaspardD 43:d2164ee3336a 777 pulseDirection_us = DIRECTION_PULSE_MIN ;
GaspardD 43:d2164ee3336a 778 }
GaspardD 21:de7a0a47f8a3 779 #if DEBUG > 1
GaspardD 43:d2164ee3336a 780 pc.printf("PWM Direction pulse: %.4lf\r\n",pulseDirection_us);
GaspardD 21:de7a0a47f8a3 781 #endif
GaspardD 43:d2164ee3336a 782 PwmDirection.pulsewidth_us(pulseDirection_us);
GaspardD 43:d2164ee3336a 783 return;
GaspardD 43:d2164ee3336a 784 }
GaspardD 8:1d8c3ca5e508 785 }
GaspardD 8:1d8c3ca5e508 786
GaspardD 11:bc24b3ba51a9 787 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 788 void obstacleOutput(void)
GaspardD 8:1d8c3ca5e508 789 {
GaspardD 8:1d8c3ca5e508 790 return;
GaspardD 8:1d8c3ca5e508 791 }
GaspardD 11:bc24b3ba51a9 792 #endif
GaspardD 11:bc24b3ba51a9 793 void sectionOutput(void)
GaspardD 8:1d8c3ca5e508 794 {
GaspardD 14:d471faa7d1a2 795 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 796 pc.printf("\r\n Output Section\r\n");
GaspardD 14:d471faa7d1a2 797 #endif
GaspardD 43:d2164ee3336a 798 if(p_sectionCourante !=NULL) {
GaspardD 43:d2164ee3336a 799 switch (st_currentSection) {
GaspardD 43:d2164ee3336a 800 case RUNNING_SECTION:
GaspardD 43:d2164ee3336a 801 break;
GaspardD 43:d2164ee3336a 802 case LOADING_SECTION:
GaspardD 43:d2164ee3336a 803 p_sectionCourante=p_sectionCourante->nextSection;
GaspardD 43:d2164ee3336a 804 tachySectionDist_cm = 0;
GaspardD 43:d2164ee3336a 805 break;
GaspardD 43:d2164ee3336a 806 case ARRET:
GaspardD 43:d2164ee3336a 807 //on est à l'arret
GaspardD 43:d2164ee3336a 808 break;
GaspardD 43:d2164ee3336a 809 default:
GaspardD 43:d2164ee3336a 810 break;
GaspardD 43:d2164ee3336a 811 }
GaspardD 11:bc24b3ba51a9 812 }
GaspardD 8:1d8c3ca5e508 813 return;
GaspardD 8:1d8c3ca5e508 814 }
GaspardD 8:1d8c3ca5e508 815
GaspardD 11:bc24b3ba51a9 816
GaspardD 11:bc24b3ba51a9 817 void maxSpeedOutput(void)
GaspardD 8:1d8c3ca5e508 818 {
GaspardD 14:d471faa7d1a2 819 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 820 pc.printf("\r\n Output MAX SPEED\r\n");
GaspardD 14:d471faa7d1a2 821 #endif
GaspardD 43:d2164ee3336a 822 if(p_sectionCourante !=NULL) {
GaspardD 43:d2164ee3336a 823 switch(st_maxSpeed) {
GaspardD 43:d2164ee3336a 824 case BLOCKED:
GaspardD 43:d2164ee3336a 825 maxSpeed_cmps = 0;
GaspardD 43:d2164ee3336a 826 break;
GaspardD 43:d2164ee3336a 827 case SPEED_WARNING:
GaspardD 43:d2164ee3336a 828 maxSpeed_cmps = p_sectionCourante->slowSpeed_cmps;
GaspardD 43:d2164ee3336a 829 break;
GaspardD 43:d2164ee3336a 830 default:
GaspardD 43:d2164ee3336a 831 maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps;
GaspardD 43:d2164ee3336a 832 break;
GaspardD 43:d2164ee3336a 833 }
GaspardD 43:d2164ee3336a 834 } else {
GaspardD 43:d2164ee3336a 835 maxSpeed_cmps = 0.0;
GaspardD 11:bc24b3ba51a9 836 }
GaspardD 8:1d8c3ca5e508 837 return;
GaspardD 8:1d8c3ca5e508 838 }
GaspardD 11:bc24b3ba51a9 839
GaspardD 11:bc24b3ba51a9 840 void throttleOutput(void)
GaspardD 11:bc24b3ba51a9 841 {
GaspardD 14:d471faa7d1a2 842 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 843 pc.printf("\r\n Output TROTTLE\r\n");
GaspardD 14:d471faa7d1a2 844 #endif
GaspardD 11:bc24b3ba51a9 845 switch (st_thro) {
GaspardD 21:de7a0a47f8a3 846 case REGULATION_SPEED:
GaspardD 22:26fc6e6f7a55 847 pulseSpeed_us = maxSpeed_cmps * 279 / 2048 + 1558 ;
GaspardD 19:771bf61be276 848 break;
GaspardD 19:771bf61be276 849 case BRAKING:
GaspardD 16:63690703b5b6 850 #if DEBUG > 2
GaspardD 19:771bf61be276 851 pc.printf("BRAKINGGGGGGGGGGGGGGGGGG !!! \r\n");
GaspardD 19:771bf61be276 852 #endif
GaspardD 19:771bf61be276 853 pulseSpeed_us = BRAKING_PULSE_US;
GaspardD 19:771bf61be276 854 break;
GaspardD 19:771bf61be276 855 case STOPPED:
GaspardD 19:771bf61be276 856 #if DEBUG > 2
GaspardD 19:771bf61be276 857 pc.printf("STOPPED\r\n");
GaspardD 16:63690703b5b6 858 #endif
GaspardD 19:771bf61be276 859 pulseSpeed_us = ZERO_PULSE_SPEED_US;
GaspardD 19:771bf61be276 860 break;
GaspardD 19:771bf61be276 861 default:
GaspardD 19:771bf61be276 862 break;
GaspardD 11:bc24b3ba51a9 863 }
GaspardD 22:26fc6e6f7a55 864
GaspardD 19:771bf61be276 865 PwmMotor.pulsewidth_us(pulseSpeed_us);
GaspardD 15:129f205ff030 866 #if DEBUG > 1
ohlimi2 36:bccddd02966a 867 pc.printf("PWM Thro pulse: %.4lf micros\r\n",pulseSpeed_us);
GaspardD 13:af9a59ccf60b 868 #endif
GaspardD 11:bc24b3ba51a9 869 return;
GaspardD 11:bc24b3ba51a9 870 }