Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@27:f8c3f1524a64, 2018-09-17 (annotated)

Committer:
GaspardD
Date:
Mon Sep 17 11:00:56 2018 +0000
Revision:
27:f8c3f1524a64
Parent:
26:3b144f243646
Child:
29:fc984fe08ca7
sans freinage adaptatif

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