Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@15:129f205ff030, 2018-09-11 (annotated)

Committer:
GaspardD
Date:
Tue Sep 11 12:28:37 2018 +0000
Revision:
15:129f205ff030
Parent:
14:d471faa7d1a2
Child:
16:63690703b5b6
section 1 unique parcours 5m

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