Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@22:26fc6e6f7a55, 2018-09-15 (annotated)

Committer:
GaspardD
Date:
Sat Sep 15 15:56:01 2018 +0000
Revision:
22:26fc6e6f7a55
Parent:
21:de7a0a47f8a3
Child:
23:04d393220daa
avec log

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