Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@12:51b1b40cc017, 2018-09-10 (annotated)

Committer:
GaspardD
Date:
Mon Sep 10 20:23:03 2018 +0000
Revision:
12:51b1b40cc017
Parent:
11:bc24b3ba51a9
Child:
13:af9a59ccf60b
complile propre

Who changed what in which revision?

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