Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@50:44c0a075e78d, 2018-09-21 (annotated)

Committer:
ohlimi2
Date:
Fri Sep 21 12:53:52 2018 +0000
Revision:
50:44c0a075e78d
Parent:
47:b17061738568 
probleme de demarage

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