Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@37:810cdbcbbf3f, 2018-09-19 (annotated)

Committer:
ohlimi2
Date:
Wed Sep 19 11:05:05 2018 +0000
Revision:
37:810cdbcbbf3f
Parent:
36:bccddd02966a
Child:
38:dba82d8b08e2
ajout du coef de la vitesse

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