Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@21:de7a0a47f8a3, 2018-09-15 (annotated)

Committer:
GaspardD
Date:
Sat Sep 15 12:58:47 2018 +0000
Revision:
21:de7a0a47f8a3
Parent:
20:017ec2428e06
Child:
22:26fc6e6f7a55
roule en arri?re ...

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