Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@48:df2b4b6feceb, 2018-09-20 (annotated)

Committer:
GaspardD
Date:
Thu Sep 20 18:56:13 2018 +0000
Revision:
48:df2b4b6feceb
Parent:
45:7d67809bd7bf
Child:
49:6a939e2ffb32
init on button

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