Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@35:f202f7e4237a, 2018-09-18 (annotated)

Committer:
GaspardD
Date:
Tue Sep 18 21:05:29 2018 +0000
Revision:
35:f202f7e4237a
Parent:
34:c98fd59c690a
Child:
36:bccddd02966a
cast int

Who changed what in which revision?

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