Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@20:017ec2428e06, 2018-09-13 (annotated)

Committer:
GaspardD
Date:
Thu Sep 13 23:09:18 2018 +0000
Revision:
20:017ec2428e06
Parent:
19:771bf61be276
Child:
21:de7a0a47f8a3
oubli? un %d

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