Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@14:d471faa7d1a2, 2018-09-11 (annotated)

Committer:
GaspardD
Date:
Tue Sep 11 09:55:49 2018 +0000
Revision:
14:d471faa7d1a2
Parent:
13:af9a59ccf60b
Child:
15:129f205ff030
pid a r?gler

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GaspardD 12:51b1b40cc017 1 #include "stateMachines.h"
GaspardD 13:af9a59ccf60b 2 #ifdef DEBUG
GaspardD 13:af9a59ccf60b 3 Serial pc(USBTX, USBRX); // tx, rx
GaspardD 13:af9a59ccf60b 4 #endif
GaspardD 11:bc24b3ba51a9 5 uint16_t distMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne
GaspardD 11:bc24b3ba51a9 6 uint16_t distMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit pour moyenne
GaspardD 11:bc24b3ba51a9 7 uint16_t distMurG45[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 45deg pour moyenne
GaspardD 11:bc24b3ba51a9 8 uint16_t distMurD45[NB_ECHANTILLONS_IR];//buffer tournant ir avant droit 45deg pour moyenne
GaspardD 11:bc24b3ba51a9 9 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 10 uint16_t distMurG10[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 10deg pour moyenne
GaspardD 11:bc24b3ba51a9 11 uint16_t distMurD10[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit 10deg pour moyenne
GaspardD 11:bc24b3ba51a9 12 uint16_t distMurFront[NB_ECHANTILLONS_IR];//buffer tournant ir front
GaspardD 11:bc24b3ba51a9 13 #endif
GaspardD 12:51b1b40cc017 14 uint16_t distMurG90Moy;
GaspardD 12:51b1b40cc017 15 uint16_t distMurD90Moy;
GaspardD 12:51b1b40cc017 16 uint16_t distMurG45Moy;
GaspardD 12:51b1b40cc017 17 uint16_t distMurD45Moy;
GaspardD 12:51b1b40cc017 18 #ifdef DLVV
GaspardD 12:51b1b40cc017 19 uint16_t distMurG10Moy;
GaspardD 12:51b1b40cc017 20 uint16_t distMurD10Moy;
GaspardD 12:51b1b40cc017 21 uint16_t distMurFrontMoy
GaspardD 12:51b1b40cc017 22
GaspardD 12:51b1b40cc017 23
GaspardD 12:51b1b40cc017 24
GaspardD 12:51b1b40cc017 25 #endif
GaspardD 8:1d8c3ca5e508 26 int dist_obstacle_lidar[NB_ECHANTILLONS_LIDAR];//buffer tournant
GaspardD 8:1d8c3ca5e508 27 int index_fifo_ir = 0;//pour géreer le buffer tournant
GaspardD 8:1d8c3ca5e508 28 int index_fifo_lidar = 0;
GaspardD 11:bc24b3ba51a9 29 //sections
GaspardD 11:bc24b3ba51a9 30 s_Section p_section1;
GaspardD 2:fd0ffe46a87d 31
GaspardD 12:51b1b40cc017 32 //Controls
GaspardD 12:51b1b40cc017 33 PwmOut PwmMotor(PB_6); // PWM4 ch1 TIM4
GaspardD 12:51b1b40cc017 34 PwmOut PwmDirection(PB_5); // PWM3 ch2 TIM3
GaspardD 8:1d8c3ca5e508 35
GaspardD 12:51b1b40cc017 36 float pulseDirection = 0.0015;
GaspardD 12:51b1b40cc017 37 float pulseSpeed = 0.015;
GaspardD 12:51b1b40cc017 38
GaspardD 12:51b1b40cc017 39
GaspardD 12:51b1b40cc017 40 //Capteurs direction
GaspardD 8:1d8c3ca5e508 41
GaspardD 12:51b1b40cc017 42 AnalogIn anaG90(PC_1);//capteur ir coté gauche
GaspardD 12:51b1b40cc017 43 AnalogIn anaD90(PA_1);//capteur ir coté droit
GaspardD 12:51b1b40cc017 44 AnalogIn anaG45(PC_0);//capteur ir avant gauche 45 deg
GaspardD 12:51b1b40cc017 45 AnalogIn anaD45(PC_2);//capteur ir avant droit 45deg
GaspardD 11:bc24b3ba51a9 46 #ifdef DLVV
GaspardD 12:51b1b40cc017 47 AnalogIn anaDlvvG(PB_0);//capteur ir avant droit 10 deg
GaspardD 12:51b1b40cc017 48 AnalogIn anaDlvvD(PC_3);//capteur ir coté droit 10 deg
GaspardD 12:51b1b40cc017 49 AnalogIn anaDlvvFront(PA_4);//capteur ir avant
GaspardD 11:bc24b3ba51a9 50 #endif
GaspardD 12:51b1b40cc017 51
GaspardD 14:d471faa7d1a2 52 int differenceGD45,differenceGD90;
GaspardD 12:51b1b40cc017 53
GaspardD 12:51b1b40cc017 54 //Capteur vitesse
GaspardD 12:51b1b40cc017 55 InterruptIn it_tachymeter(PA_11);
GaspardD 2:fd0ffe46a87d 56
GaspardD 11:bc24b3ba51a9 57
GaspardD 11:bc24b3ba51a9 58 //LIDAR
GaspardD 12:51b1b40cc017 59 Serial serialLidar(PC_10,PC_11); // tx, rx
GaspardD 11:bc24b3ba51a9 60
GaspardD 11:bc24b3ba51a9 61 int distLidar;// LiDAR actually measured distance value
GaspardD 11:bc24b3ba51a9 62 int strengthLidar;// LiDAR signal strength
GaspardD 11:bc24b3ba51a9 63 int check;// check numerical value storage
GaspardD 12:51b1b40cc017 64 int i;
GaspardD 11:bc24b3ba51a9 65 int uart[9];// store data measured by LiDAR
GaspardD 11:bc24b3ba51a9 66 const int HEADER=0x59;// data package frame header
GaspardD 11:bc24b3ba51a9 67
GaspardD 11:bc24b3ba51a9 68 //SPEED
GaspardD 11:bc24b3ba51a9 69 int maxSpeed_cmps = 0;
GaspardD 11:bc24b3ba51a9 70 int tachySpeed_cmps = 0;
GaspardD 11:bc24b3ba51a9 71 int tachyStepsRegister = 0;
GaspardD 11:bc24b3ba51a9 72 int tachySectionDist_cm = 0;
GaspardD 11:bc24b3ba51a9 73 float ms_pwmSpeedPulse = 0.0015;//IDLE
GaspardD 8:1d8c3ca5e508 74
GaspardD 8:1d8c3ca5e508 75 //Etats
GaspardD 8:1d8c3ca5e508 76 MUR_ST st_murs;
GaspardD 11:bc24b3ba51a9 77 SECTION_ST st_currentSection;
GaspardD 11:bc24b3ba51a9 78 MAX_SPEED_ST st_maxSpeed;
GaspardD 8:1d8c3ca5e508 79 THROTTLE_ST st_thro;
GaspardD 11:bc24b3ba51a9 80 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 81 OBSTACLE_ST st_obstacle;
GaspardD 11:bc24b3ba51a9 82 #endif
GaspardD 11:bc24b3ba51a9 83
GaspardD 11:bc24b3ba51a9 84 s_Section* p_sectionCourante = NULL;
GaspardD 8:1d8c3ca5e508 85
GaspardD 8:1d8c3ca5e508 86 //time monitoring
GaspardD 8:1d8c3ca5e508 87
GaspardD 11:bc24b3ba51a9 88 Timer timeSinceStart;// temps.start()/stop()/sec: read()/ms: read_ms()/µs: read_us()
GaspardD 11:bc24b3ba51a9 89 Timer timerSinceGetTachy;
GaspardD 11:bc24b3ba51a9 90
GaspardD 11:bc24b3ba51a9 91 void getTachySpeed()
GaspardD 11:bc24b3ba51a9 92 {
GaspardD 11:bc24b3ba51a9 93 //tachySteps = VALEUR_DU_PIN;
GaspardD 12:51b1b40cc017 94 //poour gérer les vitesses lentes
GaspardD 12:51b1b40cc017 95 if(tachyStepsRegister == 0 && timerSinceGetTachy.read_us() < 500000)
GaspardD 12:51b1b40cc017 96 {
GaspardD 12:51b1b40cc017 97 return;//on attend encore un peu l'aquisition de la vitesse
GaspardD 12:51b1b40cc017 98 }
GaspardD 11:bc24b3ba51a9 99 tachySectionDist_cm += tachyStepsRegister * 4;
GaspardD 14:d471faa7d1a2 100 #ifdef DEBUG
GaspardD 14:d471faa7d1a2 101 pc.printf("IT: distance parcourue %d\r\n",tachySectionDist_cm);
GaspardD 14:d471faa7d1a2 102 #endif
GaspardD 11:bc24b3ba51a9 103 tachySpeed_cmps = (tachyStepsRegister * 40000)/timerSinceGetTachy.read_us();
GaspardD 11:bc24b3ba51a9 104 tachyStepsRegister=0;
GaspardD 11:bc24b3ba51a9 105 timerSinceGetTachy.reset();
GaspardD 11:bc24b3ba51a9 106 timerSinceGetTachy.start();
GaspardD 11:bc24b3ba51a9 107 return;
GaspardD 11:bc24b3ba51a9 108 }
GaspardD 8:1d8c3ca5e508 109
GaspardD 12:51b1b40cc017 110 uint16_t getDistMoy(uint16_t* tab,int size)
GaspardD 11:bc24b3ba51a9 111 {
GaspardD 11:bc24b3ba51a9 112 int sumMoy = 0;
GaspardD 11:bc24b3ba51a9 113 for(int k=0; k<size;k++)
GaspardD 11:bc24b3ba51a9 114 {
GaspardD 11:bc24b3ba51a9 115 sumMoy+=tab[k];
GaspardD 11:bc24b3ba51a9 116 }
GaspardD 11:bc24b3ba51a9 117 return sumMoy/size;
GaspardD 11:bc24b3ba51a9 118 }
GaspardD 2:fd0ffe46a87d 119
GaspardD 12:51b1b40cc017 120 void it4cm()
GaspardD 12:51b1b40cc017 121 {
GaspardD 12:51b1b40cc017 122 tachyStepsRegister+=4;
GaspardD 14:d471faa7d1a2 123 #ifdef DEBUG
GaspardD 14:d471faa7d1a2 124 pc.printf("IT tachy\r\n");
GaspardD 14:d471faa7d1a2 125 #endif
GaspardD 12:51b1b40cc017 126 }
GaspardD 12:51b1b40cc017 127
GaspardD 12:51b1b40cc017 128 void it_serial()
GaspardD 12:51b1b40cc017 129 {
GaspardD 12:51b1b40cc017 130 if(serialLidar.getc()==HEADER) { // determine data package frame header 0x59
GaspardD 12:51b1b40cc017 131 uart[0]=HEADER;
GaspardD 12:51b1b40cc017 132 if(serialLidar.getc()==HEADER) { //determine data package frame header 0x59
GaspardD 12:51b1b40cc017 133 uart[1]=HEADER;
GaspardD 12:51b1b40cc017 134 for(i=2; i<9; i++) { // store data to array
GaspardD 12:51b1b40cc017 135 uart[i]=serialLidar.getc();
GaspardD 12:51b1b40cc017 136 }
GaspardD 12:51b1b40cc017 137 check=uart[0]+uart[1]+uart[2]+uart[3]+uart[4]+uart[5]+uart[6]+uart[7];
GaspardD 12:51b1b40cc017 138 if(uart[8]==(check&0xff)) { // check the received data as per protocols
GaspardD 12:51b1b40cc017 139 distLidar=uart[2]+uart[3]*256;// calculate distance value
GaspardD 12:51b1b40cc017 140 strengthLidar=uart[4]+uart[5]*256;// calculate signal strength value
GaspardD 12:51b1b40cc017 141 }
GaspardD 12:51b1b40cc017 142 }
GaspardD 12:51b1b40cc017 143 }
GaspardD 12:51b1b40cc017 144 }
GaspardD 8:1d8c3ca5e508 145 //########## INIT STATES MACHINES ##########
GaspardD 11:bc24b3ba51a9 146 void mursInit(void)
GaspardD 8:1d8c3ca5e508 147 {
GaspardD 13:af9a59ccf60b 148 #ifdef DEBUG
GaspardD 13:af9a59ccf60b 149 pc.baud(115200);
GaspardD 13:af9a59ccf60b 150 pc.printf("Init Murs\r\n");
GaspardD 13:af9a59ccf60b 151 #endif
GaspardD 13:af9a59ccf60b 152 timeSinceStart.start();
GaspardD 11:bc24b3ba51a9 153 st_murs=EQUILIBRAGE_REPULSIF;
GaspardD 12:51b1b40cc017 154 PwmDirection.period(0.010); //20 ms is default
GaspardD 12:51b1b40cc017 155 PwmDirection.pulsewidth(0.0015);
GaspardD 8:1d8c3ca5e508 156 return;
GaspardD 8:1d8c3ca5e508 157 }
GaspardD 11:bc24b3ba51a9 158 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 159 void obstacleInit(void)
GaspardD 8:1d8c3ca5e508 160 {
GaspardD 13:af9a59ccf60b 161 #ifdef DEBUG
GaspardD 13:af9a59ccf60b 162 pc.printf("Init Obstacle\r\n");
GaspardD 13:af9a59ccf60b 163 #endif
GaspardD 11:bc24b3ba51a9 164 st_speedLimit=FRONT_CLEAR;
GaspardD 8:1d8c3ca5e508 165 return;
GaspardD 8:1d8c3ca5e508 166 }
GaspardD 11:bc24b3ba51a9 167 #endif
GaspardD 11:bc24b3ba51a9 168 void sectionInit(void)
GaspardD 2:fd0ffe46a87d 169 {
GaspardD 13:af9a59ccf60b 170 #ifdef DEBUG
GaspardD 13:af9a59ccf60b 171 pc.printf("Init Section\r\n");
GaspardD 13:af9a59ccf60b 172 #endif
GaspardD 11:bc24b3ba51a9 173 st_currentSection=ARRET;
GaspardD 14:d471faa7d1a2 174 p_sectionCourante=&p_section1;
GaspardD 12:51b1b40cc017 175 it_tachymeter.fall(&it4cm);
GaspardD 11:bc24b3ba51a9 176 timerSinceGetTachy.start();
GaspardD 11:bc24b3ba51a9 177 getTachySpeed();//to reset
GaspardD 11:bc24b3ba51a9 178 tachySectionDist_cm = 0;
GaspardD 11:bc24b3ba51a9 179 tachyStepsRegister = 0;
GaspardD 11:bc24b3ba51a9 180
GaspardD 11:bc24b3ba51a9 181 //section de test
GaspardD 11:bc24b3ba51a9 182 p_section1.nextSection = NULL;
GaspardD 11:bc24b3ba51a9 183 p_section1.targetSpeed_cmps = 50;
GaspardD 12:51b1b40cc017 184 p_section1.slowSpeed_cmps = 15;
GaspardD 12:51b1b40cc017 185 p_section1.brakingCoefficient = 20; // application de la formule
GaspardD 12:51b1b40cc017 186 p_section1.ms_accel = 0.00005;
GaspardD 11:bc24b3ba51a9 187 p_section1.ms_decel = 0.0001;
GaspardD 11:bc24b3ba51a9 188 p_section1.lidarWarningDist_cm = 200;
GaspardD 11:bc24b3ba51a9 189 p_section1.lng_section_cm = 1000;//10m
GaspardD 14:d471faa7d1a2 190 //p_section1.coef_p = 93400000.0;
GaspardD 14:d471faa7d1a2 191 p_section1.coef_p = 25000000.0;
GaspardD 12:51b1b40cc017 192 p_section1.coef_i = 0.0;
GaspardD 12:51b1b40cc017 193 p_section1.coef_d = 0.0;
GaspardD 8:1d8c3ca5e508 194 return;
GaspardD 8:1d8c3ca5e508 195 }
GaspardD 11:bc24b3ba51a9 196
GaspardD 11:bc24b3ba51a9 197 void maxSpeedInit(void)
GaspardD 11:bc24b3ba51a9 198 {
GaspardD 13:af9a59ccf60b 199 #ifdef DEBUG
GaspardD 13:af9a59ccf60b 200 pc.printf("Init Max Speed\r\n");
GaspardD 13:af9a59ccf60b 201 #endif
GaspardD 11:bc24b3ba51a9 202 st_maxSpeed=SPEED_MAX;
GaspardD 11:bc24b3ba51a9 203 maxSpeed_cmps= p_sectionCourante->targetSpeed_cmps;
GaspardD 11:bc24b3ba51a9 204 serialLidar.baud(115200);
GaspardD 14:d471faa7d1a2 205 serialLidar.attach(&it_serial);
GaspardD 11:bc24b3ba51a9 206 return;
GaspardD 11:bc24b3ba51a9 207 }
GaspardD 11:bc24b3ba51a9 208
GaspardD 11:bc24b3ba51a9 209 void throttleInit(void)
GaspardD 8:1d8c3ca5e508 210 {
GaspardD 13:af9a59ccf60b 211 #ifdef DEBUG
GaspardD 13:af9a59ccf60b 212 pc.printf("Init Throttle\r\n");
GaspardD 13:af9a59ccf60b 213 #endif
GaspardD 8:1d8c3ca5e508 214 st_thro = AT_SPEED;
GaspardD 12:51b1b40cc017 215 PwmMotor.period(0.020); //20 ms is default
GaspardD 12:51b1b40cc017 216 PwmMotor.pulsewidth(0.0010);
GaspardD 12:51b1b40cc017 217 wait(3);
GaspardD 12:51b1b40cc017 218 PwmMotor.pulsewidth(0.0020);
GaspardD 12:51b1b40cc017 219 wait(1);
GaspardD 12:51b1b40cc017 220 PwmMotor.pulsewidth(0.0015);
GaspardD 12:51b1b40cc017 221 wait(1);
GaspardD 13:af9a59ccf60b 222 #ifdef DEBUG
GaspardD 13:af9a59ccf60b 223 pc.printf("temps init: %d micros\r\n",timeSinceStart.read_us());
GaspardD 13:af9a59ccf60b 224 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 225 timeSinceStart.reset();
GaspardD 13:af9a59ccf60b 226 timeSinceStart.start();
GaspardD 13:af9a59ccf60b 227 #endif
GaspardD 8:1d8c3ca5e508 228 return;
GaspardD 8:1d8c3ca5e508 229 }
GaspardD 2:fd0ffe46a87d 230
GaspardD 8:1d8c3ca5e508 231
GaspardD 8:1d8c3ca5e508 232 //########## UPDATE STATES ##########
GaspardD 11:bc24b3ba51a9 233 void mursUpdate(void)
GaspardD 2:fd0ffe46a87d 234 {
GaspardD 14:d471faa7d1a2 235 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 236 pc.printf("\r\nUpdate MURS\r\n");
GaspardD 14:d471faa7d1a2 237 #endif
GaspardD 11:bc24b3ba51a9 238 MUR_ST st_tmpMurs;
GaspardD 11:bc24b3ba51a9 239 //lectures
GaspardD 11:bc24b3ba51a9 240 distMurG90[index_fifo_ir]=anaG90.read_u16();
GaspardD 11:bc24b3ba51a9 241 distMurD90[index_fifo_ir]=anaD90.read_u16();
GaspardD 11:bc24b3ba51a9 242 distMurG45[index_fifo_ir]=anaG45.read_u16();
GaspardD 11:bc24b3ba51a9 243 distMurD45[index_fifo_ir]=anaD45.read_u16();
GaspardD 11:bc24b3ba51a9 244 index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR;
GaspardD 8:1d8c3ca5e508 245
GaspardD 12:51b1b40cc017 246 distMurG45Moy = getDistMoy(distMurG45,NB_ECHANTILLONS_IR);
GaspardD 12:51b1b40cc017 247 distMurD45Moy = getDistMoy(distMurD45,NB_ECHANTILLONS_IR);
GaspardD 14:d471faa7d1a2 248
GaspardD 14:d471faa7d1a2 249 #if DEBUG > 1
GaspardD 14:d471faa7d1a2 250 pc.printf("dist45G:%d dist45D:%d Deadzone: %d\r\n",distMurG45Moy,distMurD45Moy,IR_DEADZONE_U16_22cm);
GaspardD 14:d471faa7d1a2 251 #endif
GaspardD 12:51b1b40cc017 252
GaspardD 11:bc24b3ba51a9 253 switch (st_murs) {
GaspardD 11:bc24b3ba51a9 254 case EQUILIBRAGE_REPULSIF:
GaspardD 12:51b1b40cc017 255 distMurG90Moy = getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
GaspardD 12:51b1b40cc017 256 distMurD90Moy = getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
GaspardD 12:51b1b40cc017 257 if( distMurG90Moy <= IR_DEADZONE_U16_22cm)
GaspardD 11:bc24b3ba51a9 258 {
GaspardD 11:bc24b3ba51a9 259 st_tmpMurs = ATTRACTIF_D;
GaspardD 12:51b1b40cc017 260 }else if(distMurD90Moy <= IR_DEADZONE_U16_22cm)
GaspardD 11:bc24b3ba51a9 261 {
GaspardD 11:bc24b3ba51a9 262 st_tmpMurs = ATTRACTIF_G;
GaspardD 11:bc24b3ba51a9 263 }else
GaspardD 11:bc24b3ba51a9 264 {
GaspardD 11:bc24b3ba51a9 265 st_tmpMurs = EQUILIBRAGE_REPULSIF;
GaspardD 11:bc24b3ba51a9 266 }
GaspardD 11:bc24b3ba51a9 267 break;
GaspardD 11:bc24b3ba51a9 268 case ATTRACTIF_D:
GaspardD 12:51b1b40cc017 269 distMurD90Moy= getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
GaspardD 12:51b1b40cc017 270 if( distMurD90Moy >= IR_FAR_U16_105cm)
GaspardD 11:bc24b3ba51a9 271 {
GaspardD 11:bc24b3ba51a9 272 st_tmpMurs = ATTRACTIF_D;
GaspardD 11:bc24b3ba51a9 273 }else
GaspardD 11:bc24b3ba51a9 274 {
GaspardD 11:bc24b3ba51a9 275 st_tmpMurs = EQUILIBRAGE_REPULSIF;
GaspardD 11:bc24b3ba51a9 276 }
GaspardD 11:bc24b3ba51a9 277 break;
GaspardD 11:bc24b3ba51a9 278 case ATTRACTIF_G:
GaspardD 12:51b1b40cc017 279 distMurG90Moy= getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
GaspardD 12:51b1b40cc017 280 if(distMurG90Moy >= IR_FAR_U16_105cm)
GaspardD 11:bc24b3ba51a9 281 {
GaspardD 11:bc24b3ba51a9 282 st_tmpMurs = ATTRACTIF_G;
GaspardD 11:bc24b3ba51a9 283 }else
GaspardD 11:bc24b3ba51a9 284 {
GaspardD 11:bc24b3ba51a9 285 st_tmpMurs = EQUILIBRAGE_REPULSIF;
GaspardD 11:bc24b3ba51a9 286 }
GaspardD 11:bc24b3ba51a9 287 break;
GaspardD 11:bc24b3ba51a9 288 default:
GaspardD 11:bc24b3ba51a9 289 return;
GaspardD 8:1d8c3ca5e508 290 }
GaspardD 11:bc24b3ba51a9 291 st_murs = st_tmpMurs;
GaspardD 8:1d8c3ca5e508 292 return;
GaspardD 8:1d8c3ca5e508 293 }
GaspardD 11:bc24b3ba51a9 294 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 295 void obstacleUpdate(void)
GaspardD 8:1d8c3ca5e508 296 {
GaspardD 8:1d8c3ca5e508 297 return;
GaspardD 8:1d8c3ca5e508 298 }
GaspardD 11:bc24b3ba51a9 299 #endif
GaspardD 11:bc24b3ba51a9 300
GaspardD 11:bc24b3ba51a9 301 void sectionUpdate(void)
GaspardD 8:1d8c3ca5e508 302 {
GaspardD 14:d471faa7d1a2 303 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 304 pc.printf("\r\nUpdate Section\r\n");
GaspardD 14:d471faa7d1a2 305 #endif
GaspardD 11:bc24b3ba51a9 306 SECTION_ST st_tmpSection;
GaspardD 11:bc24b3ba51a9 307 switch (st_currentSection) {
GaspardD 11:bc24b3ba51a9 308 case RUNNING_SECTION:
GaspardD 11:bc24b3ba51a9 309 if(tachySectionDist_cm > p_sectionCourante->lng_section_cm)//on pourrait rajouter un test lidar
GaspardD 11:bc24b3ba51a9 310 {
GaspardD 11:bc24b3ba51a9 311 st_tmpSection = LOADING_SECTION;
GaspardD 11:bc24b3ba51a9 312 }
GaspardD 14:d471faa7d1a2 313 else
GaspardD 14:d471faa7d1a2 314 {
GaspardD 14:d471faa7d1a2 315 return;
GaspardD 14:d471faa7d1a2 316 }
GaspardD 11:bc24b3ba51a9 317 break;
GaspardD 11:bc24b3ba51a9 318 case LOADING_SECTION:
GaspardD 11:bc24b3ba51a9 319 if(p_sectionCourante->nextSection != NULL)
GaspardD 11:bc24b3ba51a9 320 {
GaspardD 11:bc24b3ba51a9 321 st_tmpSection = RUNNING_SECTION;
GaspardD 11:bc24b3ba51a9 322 }else
GaspardD 11:bc24b3ba51a9 323 {
GaspardD 11:bc24b3ba51a9 324 st_tmpSection=ARRET;
GaspardD 11:bc24b3ba51a9 325 }
GaspardD 11:bc24b3ba51a9 326 break;
GaspardD 11:bc24b3ba51a9 327 case ARRET:
GaspardD 14:d471faa7d1a2 328 if(p_sectionCourante != NULL)
GaspardD 11:bc24b3ba51a9 329 {
GaspardD 11:bc24b3ba51a9 330 st_tmpSection = RUNNING_SECTION;
GaspardD 11:bc24b3ba51a9 331 }
GaspardD 11:bc24b3ba51a9 332 else
GaspardD 11:bc24b3ba51a9 333 {
GaspardD 11:bc24b3ba51a9 334 return;
GaspardD 11:bc24b3ba51a9 335 }
GaspardD 11:bc24b3ba51a9 336 break;
GaspardD 11:bc24b3ba51a9 337 default:
GaspardD 11:bc24b3ba51a9 338 break;
GaspardD 11:bc24b3ba51a9 339 }
GaspardD 11:bc24b3ba51a9 340 st_currentSection = st_tmpSection;
GaspardD 8:1d8c3ca5e508 341 return;
GaspardD 8:1d8c3ca5e508 342 }
GaspardD 8:1d8c3ca5e508 343
GaspardD 11:bc24b3ba51a9 344 void maxSpeedUpdate(void)
GaspardD 8:1d8c3ca5e508 345 {
GaspardD 14:d471faa7d1a2 346 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 347 pc.printf("\r\nUpdate MaxSpeed\r\n");
GaspardD 14:d471faa7d1a2 348 #endif
GaspardD 11:bc24b3ba51a9 349 MAX_SPEED_ST st_tmpMaxSpeed;
GaspardD 11:bc24b3ba51a9 350 i=0;
GaspardD 11:bc24b3ba51a9 351
GaspardD 11:bc24b3ba51a9 352 if( strengthLidar > (LIDAR_STRENGTH_THRESOLD + LIDAR_STRENGH_DELTA) )
GaspardD 11:bc24b3ba51a9 353 {
GaspardD 11:bc24b3ba51a9 354 if(distLidar > 30 && distLidar < p_sectionCourante->lidarWarningDist_cm)
GaspardD 11:bc24b3ba51a9 355 {
GaspardD 11:bc24b3ba51a9 356 st_tmpMaxSpeed = SPEED_VARIABLE;
GaspardD 11:bc24b3ba51a9 357 }else
GaspardD 11:bc24b3ba51a9 358 {
GaspardD 11:bc24b3ba51a9 359 st_tmpMaxSpeed = SPEED_LIMITED;
GaspardD 11:bc24b3ba51a9 360 }
GaspardD 11:bc24b3ba51a9 361 }else if(strengthLidar < (LIDAR_STRENGTH_THRESOLD - LIDAR_STRENGH_DELTA))
GaspardD 11:bc24b3ba51a9 362 {
GaspardD 11:bc24b3ba51a9 363 st_tmpMaxSpeed = SPEED_MAX;
GaspardD 11:bc24b3ba51a9 364 }
GaspardD 11:bc24b3ba51a9 365 else{
GaspardD 11:bc24b3ba51a9 366 return;//pour ne pas changer sans arret d'etats lorsque strengthLidar est à la limite du seuil
GaspardD 11:bc24b3ba51a9 367 }
GaspardD 11:bc24b3ba51a9 368
GaspardD 11:bc24b3ba51a9 369
GaspardD 11:bc24b3ba51a9 370 st_maxSpeed = st_tmpMaxSpeed;
GaspardD 11:bc24b3ba51a9 371 return;
GaspardD 11:bc24b3ba51a9 372 }
GaspardD 11:bc24b3ba51a9 373
GaspardD 11:bc24b3ba51a9 374 void throttleUpdate(void)
GaspardD 11:bc24b3ba51a9 375 {
GaspardD 14:d471faa7d1a2 376 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 377 pc.printf("\r\nUpdate Throttle\r\n");
GaspardD 14:d471faa7d1a2 378 #endif
GaspardD 11:bc24b3ba51a9 379 THROTTLE_ST st_tmpThro;
GaspardD 14:d471faa7d1a2 380 getTachySpeed();
GaspardD 11:bc24b3ba51a9 381
GaspardD 11:bc24b3ba51a9 382 switch (st_thro) {
GaspardD 11:bc24b3ba51a9 383 case UNDER_SPEED:
GaspardD 11:bc24b3ba51a9 384 if( tachySpeed_cmps > maxSpeed_cmps )
GaspardD 11:bc24b3ba51a9 385 {
GaspardD 11:bc24b3ba51a9 386 st_tmpThro = AT_SPEED;
GaspardD 11:bc24b3ba51a9 387 }else
GaspardD 11:bc24b3ba51a9 388 {
GaspardD 11:bc24b3ba51a9 389 return;
GaspardD 11:bc24b3ba51a9 390 }
GaspardD 11:bc24b3ba51a9 391 break;
GaspardD 11:bc24b3ba51a9 392 case OVER_SPEED:
GaspardD 11:bc24b3ba51a9 393 if( tachySpeed_cmps < maxSpeed_cmps )
GaspardD 11:bc24b3ba51a9 394 {
GaspardD 11:bc24b3ba51a9 395 st_tmpThro = AT_SPEED;
GaspardD 11:bc24b3ba51a9 396 }else
GaspardD 11:bc24b3ba51a9 397 {
GaspardD 11:bc24b3ba51a9 398 return;
GaspardD 11:bc24b3ba51a9 399 }
GaspardD 11:bc24b3ba51a9 400 break;
GaspardD 11:bc24b3ba51a9 401 case AT_SPEED:
GaspardD 11:bc24b3ba51a9 402 if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS) )
GaspardD 11:bc24b3ba51a9 403 {
GaspardD 11:bc24b3ba51a9 404 st_tmpThro = OVER_SPEED;
GaspardD 11:bc24b3ba51a9 405 } else if( tachySpeed_cmps < (maxSpeed_cmps - SPEED_DELTA_CMPS) )
GaspardD 11:bc24b3ba51a9 406 {
GaspardD 11:bc24b3ba51a9 407 st_tmpThro = UNDER_SPEED;
GaspardD 11:bc24b3ba51a9 408 }else
GaspardD 11:bc24b3ba51a9 409 {
GaspardD 11:bc24b3ba51a9 410 return;
GaspardD 11:bc24b3ba51a9 411 }
GaspardD 11:bc24b3ba51a9 412 break;
GaspardD 11:bc24b3ba51a9 413 default:
GaspardD 11:bc24b3ba51a9 414 break;
GaspardD 11:bc24b3ba51a9 415 }
GaspardD 11:bc24b3ba51a9 416
GaspardD 11:bc24b3ba51a9 417 st_thro = st_tmpThro;
GaspardD 8:1d8c3ca5e508 418 return;
GaspardD 8:1d8c3ca5e508 419 }
GaspardD 2:fd0ffe46a87d 420
GaspardD 8:1d8c3ca5e508 421 //########## OUTPUT STATES ##########
GaspardD 11:bc24b3ba51a9 422 //updating output parameters
GaspardD 11:bc24b3ba51a9 423 void mursOutput(void)
GaspardD 12:51b1b40cc017 424 {
GaspardD 14:d471faa7d1a2 425 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 426 pc.printf("\r\n Output MURS\r\n");
GaspardD 14:d471faa7d1a2 427 #endif
GaspardD 11:bc24b3ba51a9 428 switch (st_murs) {
GaspardD 12:51b1b40cc017 429 case EQUILIBRAGE_REPULSIF://TO DO TODO
GaspardD 11:bc24b3ba51a9 430 case ATTRACTIF_G:
GaspardD 11:bc24b3ba51a9 431 case ATTRACTIF_D:
GaspardD 14:d471faa7d1a2 432
GaspardD 14:d471faa7d1a2 433 differenceGD90 = distMurD90Moy - distMurG90Moy;
GaspardD 12:51b1b40cc017 434 differenceGD45 = distMurD45Moy - distMurG45Moy;
GaspardD 14:d471faa7d1a2 435 pulseDirection = (double) ((double)((differenceGD45+differenceGD45)/2) / (double)p_sectionCourante->coef_p) + 0.0015;
GaspardD 11:bc24b3ba51a9 436 default:
GaspardD 11:bc24b3ba51a9 437 break;
GaspardD 11:bc24b3ba51a9 438 }
GaspardD 14:d471faa7d1a2 439 #if DEBUG > 1
GaspardD 14:d471faa7d1a2 440 pc.printf("PWM pulse: %.7f\r\n",pulseDirection);
GaspardD 14:d471faa7d1a2 441 #endif
GaspardD 12:51b1b40cc017 442 PwmDirection.pulsewidth(pulseDirection);
GaspardD 8:1d8c3ca5e508 443 return;
GaspardD 8:1d8c3ca5e508 444 }
GaspardD 8:1d8c3ca5e508 445
GaspardD 11:bc24b3ba51a9 446 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 447 void obstacleOutput(void)
GaspardD 8:1d8c3ca5e508 448 {
GaspardD 8:1d8c3ca5e508 449 return;
GaspardD 8:1d8c3ca5e508 450 }
GaspardD 11:bc24b3ba51a9 451 #endif
GaspardD 11:bc24b3ba51a9 452 void sectionOutput(void)
GaspardD 8:1d8c3ca5e508 453 {
GaspardD 14:d471faa7d1a2 454 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 455 pc.printf("\r\n Output Section\r\n");
GaspardD 14:d471faa7d1a2 456 #endif
GaspardD 11:bc24b3ba51a9 457 switch (st_currentSection) {
GaspardD 11:bc24b3ba51a9 458 case RUNNING_SECTION:
GaspardD 11:bc24b3ba51a9 459 break;
GaspardD 11:bc24b3ba51a9 460 case LOADING_SECTION:
GaspardD 11:bc24b3ba51a9 461 p_sectionCourante=p_sectionCourante->nextSection;
GaspardD 11:bc24b3ba51a9 462 tachySectionDist_cm = 0;
GaspardD 11:bc24b3ba51a9 463 break;
GaspardD 11:bc24b3ba51a9 464 case ARRET:
GaspardD 11:bc24b3ba51a9 465 wait(10);//on est à l'arret
GaspardD 11:bc24b3ba51a9 466 break;
GaspardD 11:bc24b3ba51a9 467 default:
GaspardD 11:bc24b3ba51a9 468 break;
GaspardD 11:bc24b3ba51a9 469 }
GaspardD 8:1d8c3ca5e508 470 return;
GaspardD 8:1d8c3ca5e508 471 }
GaspardD 8:1d8c3ca5e508 472
GaspardD 11:bc24b3ba51a9 473
GaspardD 11:bc24b3ba51a9 474 void maxSpeedOutput(void)
GaspardD 8:1d8c3ca5e508 475 {
GaspardD 14:d471faa7d1a2 476 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 477 pc.printf("\r\n Output MAX SPEED\r\n");
GaspardD 14:d471faa7d1a2 478 #endif
GaspardD 11:bc24b3ba51a9 479 switch(st_maxSpeed)
GaspardD 11:bc24b3ba51a9 480 {
GaspardD 11:bc24b3ba51a9 481 case SPEED_LIMITED:
GaspardD 11:bc24b3ba51a9 482 maxSpeed_cmps = p_sectionCourante->slowSpeed_cmps;
GaspardD 11:bc24b3ba51a9 483 break;
GaspardD 11:bc24b3ba51a9 484 case SPEED_VARIABLE:
GaspardD 11:bc24b3ba51a9 485 maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps * (distLidar/(distLidar+p_sectionCourante->brakingCoefficient));
GaspardD 11:bc24b3ba51a9 486 break;
GaspardD 11:bc24b3ba51a9 487 default:
GaspardD 11:bc24b3ba51a9 488 maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps;
GaspardD 11:bc24b3ba51a9 489 break;
GaspardD 11:bc24b3ba51a9 490 }
GaspardD 8:1d8c3ca5e508 491 return;
GaspardD 8:1d8c3ca5e508 492 }
GaspardD 11:bc24b3ba51a9 493
GaspardD 11:bc24b3ba51a9 494 void throttleOutput(void)
GaspardD 11:bc24b3ba51a9 495 {
GaspardD 14:d471faa7d1a2 496 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 497 pc.printf("\r\n Output TROTTLE\r\n");
GaspardD 14:d471faa7d1a2 498 #endif
GaspardD 11:bc24b3ba51a9 499 switch (st_thro) {
GaspardD 11:bc24b3ba51a9 500 case UNDER_SPEED:
GaspardD 11:bc24b3ba51a9 501 ms_pwmSpeedPulse += p_sectionCourante->ms_accel;
GaspardD 11:bc24b3ba51a9 502 break;
GaspardD 11:bc24b3ba51a9 503 case OVER_SPEED:
GaspardD 11:bc24b3ba51a9 504 ms_pwmSpeedPulse -= p_sectionCourante->ms_decel;
GaspardD 11:bc24b3ba51a9 505 break;
GaspardD 11:bc24b3ba51a9 506 case AT_SPEED:
GaspardD 11:bc24b3ba51a9 507 default:
GaspardD 11:bc24b3ba51a9 508 break;
GaspardD 11:bc24b3ba51a9 509 }
GaspardD 14:d471faa7d1a2 510 #if DEBUG > 0
GaspardD 13:af9a59ccf60b 511 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 13:af9a59ccf60b 512 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 14:d471faa7d1a2 513 wait(2);
GaspardD 13:af9a59ccf60b 514 timeSinceStart.reset();
GaspardD 13:af9a59ccf60b 515 timeSinceStart.start();
GaspardD 13:af9a59ccf60b 516 #endif
GaspardD 11:bc24b3ba51a9 517 return;
GaspardD 11:bc24b3ba51a9 518 }