Hlimi Omar / Mbed 2 deprecated biniou

TRR2018 omar

Dependencies:   mbed

Fork of biniou by TRR 2018

stateMachines.cpp@18:38504337d2fc, 2018-09-12 (annotated)

Committer:
GaspardD
Date:
Wed Sep 12 22:37:07 2018 +0000
Revision:
18:38504337d2fc
Parent:
17:8c465656eea4
Child:
19:771bf61be276
fonctionne de mani?re tr?s propre; d?couverte: m?me ? en lui disant d'aller ? la vitesse minimale en proportionnel, la vitesse du robot d?passe toujours la consigne

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 17:8c465656eea4 33 int pulseDirection = DIRECTION_PULSE_MIDDLE;
GaspardD 17:8c465656eea4 34 int pulseSpeed = INITAL_PULSE_SPEED;
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 17:8c465656eea4 150 PwmDirection.period_us(SPEED_PERIOD);
GaspardD 17:8c465656eea4 151 PwmDirection.pulsewidth_us(DIRECTION_PULSE_MIDDLE);// milieu
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 18:38504337d2fc 179 p_section1.targetSpeed_cmps = 10;
GaspardD 18:38504337d2fc 180 p_section1.slowSpeed_cmps = 5;
GaspardD 18:38504337d2fc 181 p_section1.brakingCoefficient = 30; // application de la formule
GaspardD 17:8c465656eea4 182 p_section1.us_accel = 1;//+1 microsec each loop
GaspardD 17:8c465656eea4 183 p_section1.us_decel = 1;
GaspardD 18:38504337d2fc 184 p_section1.lidarWarningDist_cm = 300;
GaspardD 18:38504337d2fc 185 p_section1.lng_section_cm = 1500;//1500cm
GaspardD 17:8c465656eea4 186 p_section1.coef_p = 35;
GaspardD 17:8c465656eea4 187 p_section1.coef_i = 0;
GaspardD 17:8c465656eea4 188 p_section1.coef_d = 0;
GaspardD 8:1d8c3ca5e508 189 return;
GaspardD 8:1d8c3ca5e508 190 }
GaspardD 11:bc24b3ba51a9 191
GaspardD 11:bc24b3ba51a9 192 void maxSpeedInit(void)
GaspardD 11:bc24b3ba51a9 193 {
GaspardD 13:af9a59ccf60b 194 #ifdef DEBUG
GaspardD 13:af9a59ccf60b 195 pc.printf("Init Max Speed\r\n");
GaspardD 13:af9a59ccf60b 196 #endif
GaspardD 11:bc24b3ba51a9 197 st_maxSpeed=SPEED_MAX;
GaspardD 11:bc24b3ba51a9 198 maxSpeed_cmps= p_sectionCourante->targetSpeed_cmps;
GaspardD 11:bc24b3ba51a9 199 serialLidar.baud(115200);
GaspardD 14:d471faa7d1a2 200 serialLidar.attach(&it_serial);
GaspardD 11:bc24b3ba51a9 201 return;
GaspardD 11:bc24b3ba51a9 202 }
GaspardD 11:bc24b3ba51a9 203
GaspardD 11:bc24b3ba51a9 204 void throttleInit(void)
GaspardD 8:1d8c3ca5e508 205 {
GaspardD 13:af9a59ccf60b 206 #ifdef DEBUG
GaspardD 13:af9a59ccf60b 207 pc.printf("Init Throttle\r\n");
GaspardD 13:af9a59ccf60b 208 #endif
GaspardD 16:63690703b5b6 209 st_thro = UNDER_SPEED;
GaspardD 17:8c465656eea4 210 PwmMotor.period_us(SPEED_PERIOD); //20 ms is default
GaspardD 17:8c465656eea4 211 PwmMotor.pulsewidth_us(1000);
GaspardD 12:51b1b40cc017 212 wait(3);
GaspardD 17:8c465656eea4 213 PwmMotor.pulsewidth_us(2000);
GaspardD 12:51b1b40cc017 214 wait(1);
GaspardD 17:8c465656eea4 215 PwmMotor.pulsewidth_us(1500);
GaspardD 12:51b1b40cc017 216 wait(1);
GaspardD 16:63690703b5b6 217 pulseSpeed = INITAL_PULSE_SPEED;
GaspardD 13:af9a59ccf60b 218 #ifdef DEBUG
GaspardD 13:af9a59ccf60b 219 pc.printf("temps init: %d micros\r\n",timeSinceStart.read_us());
GaspardD 13:af9a59ccf60b 220 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 221 timeSinceStart.reset();
GaspardD 13:af9a59ccf60b 222 timeSinceStart.start();
GaspardD 13:af9a59ccf60b 223 #endif
GaspardD 8:1d8c3ca5e508 224 return;
GaspardD 8:1d8c3ca5e508 225 }
GaspardD 2:fd0ffe46a87d 226
GaspardD 8:1d8c3ca5e508 227
GaspardD 8:1d8c3ca5e508 228 //########## UPDATE STATES ##########
GaspardD 11:bc24b3ba51a9 229 void mursUpdate(void)
GaspardD 2:fd0ffe46a87d 230 {
GaspardD 14:d471faa7d1a2 231 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 232 pc.printf("\r\nUpdate MURS\r\n");
GaspardD 14:d471faa7d1a2 233 #endif
GaspardD 11:bc24b3ba51a9 234 MUR_ST st_tmpMurs;
GaspardD 11:bc24b3ba51a9 235 //lectures
GaspardD 11:bc24b3ba51a9 236 distMurG90[index_fifo_ir]=anaG90.read_u16();
GaspardD 11:bc24b3ba51a9 237 distMurD90[index_fifo_ir]=anaD90.read_u16();
GaspardD 11:bc24b3ba51a9 238 distMurG45[index_fifo_ir]=anaG45.read_u16();
GaspardD 11:bc24b3ba51a9 239 distMurD45[index_fifo_ir]=anaD45.read_u16();
GaspardD 11:bc24b3ba51a9 240 index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR;
GaspardD 8:1d8c3ca5e508 241
GaspardD 12:51b1b40cc017 242 distMurG45Moy = getDistMoy(distMurG45,NB_ECHANTILLONS_IR);
GaspardD 12:51b1b40cc017 243 distMurD45Moy = getDistMoy(distMurD45,NB_ECHANTILLONS_IR);
GaspardD 14:d471faa7d1a2 244
GaspardD 14:d471faa7d1a2 245 #if DEBUG > 1
GaspardD 14:d471faa7d1a2 246 pc.printf("dist45G:%d dist45D:%d Deadzone: %d\r\n",distMurG45Moy,distMurD45Moy,IR_DEADZONE_U16_22cm);
GaspardD 14:d471faa7d1a2 247 #endif
GaspardD 12:51b1b40cc017 248
GaspardD 11:bc24b3ba51a9 249 switch (st_murs) {
GaspardD 11:bc24b3ba51a9 250 case EQUILIBRAGE_REPULSIF:
GaspardD 12:51b1b40cc017 251 distMurG90Moy = getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
GaspardD 12:51b1b40cc017 252 distMurD90Moy = getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
GaspardD 12:51b1b40cc017 253 if( distMurG90Moy <= IR_DEADZONE_U16_22cm)
GaspardD 11:bc24b3ba51a9 254 {
GaspardD 11:bc24b3ba51a9 255 st_tmpMurs = ATTRACTIF_D;
GaspardD 12:51b1b40cc017 256 }else if(distMurD90Moy <= IR_DEADZONE_U16_22cm)
GaspardD 11:bc24b3ba51a9 257 {
GaspardD 11:bc24b3ba51a9 258 st_tmpMurs = ATTRACTIF_G;
GaspardD 11:bc24b3ba51a9 259 }else
GaspardD 11:bc24b3ba51a9 260 {
GaspardD 11:bc24b3ba51a9 261 st_tmpMurs = EQUILIBRAGE_REPULSIF;
GaspardD 11:bc24b3ba51a9 262 }
GaspardD 11:bc24b3ba51a9 263 break;
GaspardD 11:bc24b3ba51a9 264 case ATTRACTIF_D:
GaspardD 12:51b1b40cc017 265 distMurD90Moy= getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
GaspardD 12:51b1b40cc017 266 if( distMurD90Moy >= IR_FAR_U16_105cm)
GaspardD 11:bc24b3ba51a9 267 {
GaspardD 11:bc24b3ba51a9 268 st_tmpMurs = ATTRACTIF_D;
GaspardD 11:bc24b3ba51a9 269 }else
GaspardD 11:bc24b3ba51a9 270 {
GaspardD 11:bc24b3ba51a9 271 st_tmpMurs = EQUILIBRAGE_REPULSIF;
GaspardD 11:bc24b3ba51a9 272 }
GaspardD 11:bc24b3ba51a9 273 break;
GaspardD 11:bc24b3ba51a9 274 case ATTRACTIF_G:
GaspardD 12:51b1b40cc017 275 distMurG90Moy= getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
GaspardD 12:51b1b40cc017 276 if(distMurG90Moy >= IR_FAR_U16_105cm)
GaspardD 11:bc24b3ba51a9 277 {
GaspardD 11:bc24b3ba51a9 278 st_tmpMurs = ATTRACTIF_G;
GaspardD 11:bc24b3ba51a9 279 }else
GaspardD 11:bc24b3ba51a9 280 {
GaspardD 11:bc24b3ba51a9 281 st_tmpMurs = EQUILIBRAGE_REPULSIF;
GaspardD 11:bc24b3ba51a9 282 }
GaspardD 11:bc24b3ba51a9 283 break;
GaspardD 11:bc24b3ba51a9 284 default:
GaspardD 11:bc24b3ba51a9 285 return;
GaspardD 8:1d8c3ca5e508 286 }
GaspardD 11:bc24b3ba51a9 287 st_murs = st_tmpMurs;
GaspardD 8:1d8c3ca5e508 288 return;
GaspardD 8:1d8c3ca5e508 289 }
GaspardD 11:bc24b3ba51a9 290 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 291 void obstacleUpdate(void)
GaspardD 8:1d8c3ca5e508 292 {
GaspardD 8:1d8c3ca5e508 293 return;
GaspardD 8:1d8c3ca5e508 294 }
GaspardD 11:bc24b3ba51a9 295 #endif
GaspardD 11:bc24b3ba51a9 296
GaspardD 11:bc24b3ba51a9 297 void sectionUpdate(void)
GaspardD 8:1d8c3ca5e508 298 {
GaspardD 14:d471faa7d1a2 299 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 300 pc.printf("\r\nUpdate Section\r\n");
GaspardD 14:d471faa7d1a2 301 #endif
GaspardD 11:bc24b3ba51a9 302 SECTION_ST st_tmpSection;
GaspardD 11:bc24b3ba51a9 303 switch (st_currentSection) {
GaspardD 11:bc24b3ba51a9 304 case RUNNING_SECTION:
GaspardD 11:bc24b3ba51a9 305 if(tachySectionDist_cm > p_sectionCourante->lng_section_cm)//on pourrait rajouter un test lidar
GaspardD 11:bc24b3ba51a9 306 {
GaspardD 11:bc24b3ba51a9 307 st_tmpSection = LOADING_SECTION;
GaspardD 11:bc24b3ba51a9 308 }
GaspardD 14:d471faa7d1a2 309 else
GaspardD 14:d471faa7d1a2 310 {
GaspardD 14:d471faa7d1a2 311 return;
GaspardD 14:d471faa7d1a2 312 }
GaspardD 11:bc24b3ba51a9 313 break;
GaspardD 11:bc24b3ba51a9 314 case LOADING_SECTION:
GaspardD 16:63690703b5b6 315 if(p_sectionCourante != NULL) //la section a ete chargee dans sectionOutput
GaspardD 11:bc24b3ba51a9 316 {
GaspardD 11:bc24b3ba51a9 317 st_tmpSection = RUNNING_SECTION;
GaspardD 11:bc24b3ba51a9 318 }else
GaspardD 11:bc24b3ba51a9 319 {
GaspardD 11:bc24b3ba51a9 320 st_tmpSection=ARRET;
GaspardD 11:bc24b3ba51a9 321 }
GaspardD 11:bc24b3ba51a9 322 break;
GaspardD 11:bc24b3ba51a9 323 case ARRET:
GaspardD 14:d471faa7d1a2 324 if(p_sectionCourante != NULL)
GaspardD 11:bc24b3ba51a9 325 {
GaspardD 11:bc24b3ba51a9 326 st_tmpSection = RUNNING_SECTION;
GaspardD 11:bc24b3ba51a9 327 }
GaspardD 11:bc24b3ba51a9 328 else
GaspardD 11:bc24b3ba51a9 329 {
GaspardD 11:bc24b3ba51a9 330 return;
GaspardD 11:bc24b3ba51a9 331 }
GaspardD 11:bc24b3ba51a9 332 break;
GaspardD 11:bc24b3ba51a9 333 default:
GaspardD 11:bc24b3ba51a9 334 break;
GaspardD 11:bc24b3ba51a9 335 }
GaspardD 11:bc24b3ba51a9 336 st_currentSection = st_tmpSection;
GaspardD 8:1d8c3ca5e508 337 return;
GaspardD 8:1d8c3ca5e508 338 }
GaspardD 8:1d8c3ca5e508 339
GaspardD 11:bc24b3ba51a9 340 void maxSpeedUpdate(void)
GaspardD 8:1d8c3ca5e508 341 {
GaspardD 14:d471faa7d1a2 342 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 343 pc.printf("\r\nUpdate MaxSpeed\r\n");
GaspardD 14:d471faa7d1a2 344 #endif
GaspardD 11:bc24b3ba51a9 345 MAX_SPEED_ST st_tmpMaxSpeed;
GaspardD 11:bc24b3ba51a9 346 i=0;
GaspardD 11:bc24b3ba51a9 347
GaspardD 11:bc24b3ba51a9 348 if( strengthLidar > (LIDAR_STRENGTH_THRESOLD + LIDAR_STRENGH_DELTA) )
GaspardD 11:bc24b3ba51a9 349 {
GaspardD 11:bc24b3ba51a9 350 if(distLidar > 30 && distLidar < p_sectionCourante->lidarWarningDist_cm)
GaspardD 11:bc24b3ba51a9 351 {
GaspardD 11:bc24b3ba51a9 352 st_tmpMaxSpeed = SPEED_VARIABLE;
GaspardD 11:bc24b3ba51a9 353 }else
GaspardD 11:bc24b3ba51a9 354 {
GaspardD 11:bc24b3ba51a9 355 st_tmpMaxSpeed = SPEED_LIMITED;
GaspardD 11:bc24b3ba51a9 356 }
GaspardD 11:bc24b3ba51a9 357 }else if(strengthLidar < (LIDAR_STRENGTH_THRESOLD - LIDAR_STRENGH_DELTA))
GaspardD 11:bc24b3ba51a9 358 {
GaspardD 11:bc24b3ba51a9 359 st_tmpMaxSpeed = SPEED_MAX;
GaspardD 11:bc24b3ba51a9 360 }
GaspardD 11:bc24b3ba51a9 361 else{
GaspardD 11:bc24b3ba51a9 362 return;//pour ne pas changer sans arret d'etats lorsque strengthLidar est à la limite du seuil
GaspardD 11:bc24b3ba51a9 363 }
GaspardD 11:bc24b3ba51a9 364
GaspardD 11:bc24b3ba51a9 365
GaspardD 11:bc24b3ba51a9 366 st_maxSpeed = st_tmpMaxSpeed;
GaspardD 11:bc24b3ba51a9 367 return;
GaspardD 11:bc24b3ba51a9 368 }
GaspardD 11:bc24b3ba51a9 369
GaspardD 11:bc24b3ba51a9 370 void throttleUpdate(void)
GaspardD 11:bc24b3ba51a9 371 {
GaspardD 14:d471faa7d1a2 372 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 373 pc.printf("\r\nUpdate Throttle\r\n");
GaspardD 14:d471faa7d1a2 374 #endif
GaspardD 11:bc24b3ba51a9 375 THROTTLE_ST st_tmpThro;
GaspardD 14:d471faa7d1a2 376 getTachySpeed();
GaspardD 11:bc24b3ba51a9 377 switch (st_thro) {
GaspardD 11:bc24b3ba51a9 378 case UNDER_SPEED:
GaspardD 16:63690703b5b6 379 if(st_currentSection == ARRET)
GaspardD 16:63690703b5b6 380 {
GaspardD 16:63690703b5b6 381 st_tmpThro = BRAKING;
GaspardD 16:63690703b5b6 382 }
GaspardD 17:8c465656eea4 383 else if( tachySpeed_cmps >= maxSpeed_cmps )
GaspardD 11:bc24b3ba51a9 384 {
GaspardD 11:bc24b3ba51a9 385 st_tmpThro = AT_SPEED;
GaspardD 11:bc24b3ba51a9 386 }else
GaspardD 11:bc24b3ba51a9 387 {
GaspardD 11:bc24b3ba51a9 388 return;
GaspardD 11:bc24b3ba51a9 389 }
GaspardD 11:bc24b3ba51a9 390 break;
GaspardD 11:bc24b3ba51a9 391 case OVER_SPEED:
GaspardD 16:63690703b5b6 392 if(st_currentSection == ARRET)
GaspardD 16:63690703b5b6 393 {
GaspardD 16:63690703b5b6 394 st_tmpThro = BRAKING;
GaspardD 16:63690703b5b6 395 }
GaspardD 17:8c465656eea4 396 else if( tachySpeed_cmps <= maxSpeed_cmps)
GaspardD 11:bc24b3ba51a9 397 {
GaspardD 11:bc24b3ba51a9 398 st_tmpThro = AT_SPEED;
GaspardD 11:bc24b3ba51a9 399 }else
GaspardD 11:bc24b3ba51a9 400 {
GaspardD 11:bc24b3ba51a9 401 return;
GaspardD 11:bc24b3ba51a9 402 }
GaspardD 11:bc24b3ba51a9 403 break;
GaspardD 11:bc24b3ba51a9 404 case AT_SPEED:
GaspardD 16:63690703b5b6 405 if(st_currentSection == ARRET)
GaspardD 16:63690703b5b6 406 {
GaspardD 16:63690703b5b6 407 st_tmpThro = BRAKING;
GaspardD 16:63690703b5b6 408 }
GaspardD 16:63690703b5b6 409 else if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS) )
GaspardD 11:bc24b3ba51a9 410 {
GaspardD 11:bc24b3ba51a9 411 st_tmpThro = OVER_SPEED;
GaspardD 11:bc24b3ba51a9 412 } else if( tachySpeed_cmps < (maxSpeed_cmps - SPEED_DELTA_CMPS) )
GaspardD 11:bc24b3ba51a9 413 {
GaspardD 11:bc24b3ba51a9 414 st_tmpThro = UNDER_SPEED;
GaspardD 16:63690703b5b6 415 }
GaspardD 16:63690703b5b6 416 else
GaspardD 11:bc24b3ba51a9 417 {
GaspardD 11:bc24b3ba51a9 418 return;
GaspardD 11:bc24b3ba51a9 419 }
GaspardD 11:bc24b3ba51a9 420 break;
GaspardD 16:63690703b5b6 421 case BRAKING:
GaspardD 16:63690703b5b6 422 if(st_currentSection == RUNNING_SECTION){
GaspardD 16:63690703b5b6 423 if( tachySpeed_cmps < maxSpeed_cmps )
GaspardD 16:63690703b5b6 424 {
GaspardD 16:63690703b5b6 425 st_tmpThro = UNDER_SPEED;
GaspardD 16:63690703b5b6 426 }else if( tachySpeed_cmps > maxSpeed_cmps )
GaspardD 16:63690703b5b6 427 {
GaspardD 16:63690703b5b6 428 st_tmpThro = OVER_SPEED;
GaspardD 16:63690703b5b6 429 }else
GaspardD 16:63690703b5b6 430 {
GaspardD 16:63690703b5b6 431 st_tmpThro = AT_SPEED;
GaspardD 16:63690703b5b6 432 }
GaspardD 16:63690703b5b6 433 }
GaspardD 16:63690703b5b6 434 else
GaspardD 16:63690703b5b6 435 {
GaspardD 16:63690703b5b6 436 return;
GaspardD 16:63690703b5b6 437 }
GaspardD 11:bc24b3ba51a9 438 default:
GaspardD 11:bc24b3ba51a9 439 break;
GaspardD 11:bc24b3ba51a9 440 }
GaspardD 11:bc24b3ba51a9 441
GaspardD 11:bc24b3ba51a9 442 st_thro = st_tmpThro;
GaspardD 8:1d8c3ca5e508 443 return;
GaspardD 8:1d8c3ca5e508 444 }
GaspardD 2:fd0ffe46a87d 445
GaspardD 8:1d8c3ca5e508 446 //########## OUTPUT STATES ##########
GaspardD 11:bc24b3ba51a9 447 //updating output parameters
GaspardD 11:bc24b3ba51a9 448 void mursOutput(void)
GaspardD 12:51b1b40cc017 449 {
GaspardD 14:d471faa7d1a2 450 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 451 pc.printf("\r\n Output MURS\r\n");
GaspardD 14:d471faa7d1a2 452 #endif
GaspardD 11:bc24b3ba51a9 453 switch (st_murs) {
GaspardD 12:51b1b40cc017 454 case EQUILIBRAGE_REPULSIF://TO DO TODO
GaspardD 11:bc24b3ba51a9 455 case ATTRACTIF_G:
GaspardD 11:bc24b3ba51a9 456 case ATTRACTIF_D:
GaspardD 14:d471faa7d1a2 457
GaspardD 14:d471faa7d1a2 458 differenceGD90 = distMurD90Moy - distMurG90Moy;
GaspardD 12:51b1b40cc017 459 differenceGD45 = distMurD45Moy - distMurG45Moy;
GaspardD 17:8c465656eea4 460 pulseDirection = ( ((differenceGD90*2+differenceGD45*4)/8) / p_sectionCourante->coef_p) + 1500;
GaspardD 17:8c465656eea4 461 if(pulseDirection > DIRECTION_PULSE_MAX)//POUR TOURNER A GAUCHE
GaspardD 15:129f205ff030 462 {
GaspardD 15:129f205ff030 463 #if DEBUG > 1
GaspardD 17:8c465656eea4 464 pc.printf("!!! OVER PWM Direction pulse: %d\r\n",pulseDirection);
GaspardD 15:129f205ff030 465 #endif
GaspardD 17:8c465656eea4 466 pulseDirection = DIRECTION_PULSE_MAX;
GaspardD 15:129f205ff030 467 }
GaspardD 17:8c465656eea4 468 else if(pulseDirection < DIRECTION_PULSE_MIN )//POUR TOURNER A DROITE
GaspardD 15:129f205ff030 469 {
GaspardD 15:129f205ff030 470 #if DEBUG > 1
GaspardD 17:8c465656eea4 471 pc.printf("!!! UNDER PWM Direction pulse: %d\r\n",pulseDirection);
GaspardD 15:129f205ff030 472 #endif
GaspardD 17:8c465656eea4 473 pulseDirection = DIRECTION_PULSE_MIN ;
GaspardD 15:129f205ff030 474 }
GaspardD 11:bc24b3ba51a9 475 default:
GaspardD 11:bc24b3ba51a9 476 break;
GaspardD 11:bc24b3ba51a9 477 }
GaspardD 14:d471faa7d1a2 478 #if DEBUG > 1
GaspardD 17:8c465656eea4 479 pc.printf("PWM Direction pulse: %d\r\n",pulseDirection);
GaspardD 14:d471faa7d1a2 480 #endif
GaspardD 17:8c465656eea4 481 PwmDirection.pulsewidth_us(pulseDirection);
GaspardD 8:1d8c3ca5e508 482 return;
GaspardD 8:1d8c3ca5e508 483 }
GaspardD 8:1d8c3ca5e508 484
GaspardD 11:bc24b3ba51a9 485 #ifdef DLVV
GaspardD 11:bc24b3ba51a9 486 void obstacleOutput(void)
GaspardD 8:1d8c3ca5e508 487 {
GaspardD 8:1d8c3ca5e508 488 return;
GaspardD 8:1d8c3ca5e508 489 }
GaspardD 11:bc24b3ba51a9 490 #endif
GaspardD 11:bc24b3ba51a9 491 void sectionOutput(void)
GaspardD 8:1d8c3ca5e508 492 {
GaspardD 14:d471faa7d1a2 493 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 494 pc.printf("\r\n Output Section\r\n");
GaspardD 14:d471faa7d1a2 495 #endif
GaspardD 11:bc24b3ba51a9 496 switch (st_currentSection) {
GaspardD 11:bc24b3ba51a9 497 case RUNNING_SECTION:
GaspardD 11:bc24b3ba51a9 498 break;
GaspardD 11:bc24b3ba51a9 499 case LOADING_SECTION:
GaspardD 11:bc24b3ba51a9 500 p_sectionCourante=p_sectionCourante->nextSection;
GaspardD 11:bc24b3ba51a9 501 tachySectionDist_cm = 0;
GaspardD 11:bc24b3ba51a9 502 break;
GaspardD 11:bc24b3ba51a9 503 case ARRET:
GaspardD 16:63690703b5b6 504 //on est à l'arret
GaspardD 11:bc24b3ba51a9 505 break;
GaspardD 11:bc24b3ba51a9 506 default:
GaspardD 11:bc24b3ba51a9 507 break;
GaspardD 11:bc24b3ba51a9 508 }
GaspardD 8:1d8c3ca5e508 509 return;
GaspardD 8:1d8c3ca5e508 510 }
GaspardD 8:1d8c3ca5e508 511
GaspardD 11:bc24b3ba51a9 512
GaspardD 11:bc24b3ba51a9 513 void maxSpeedOutput(void)
GaspardD 8:1d8c3ca5e508 514 {
GaspardD 14:d471faa7d1a2 515 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 516 pc.printf("\r\n Output MAX SPEED\r\n");
GaspardD 14:d471faa7d1a2 517 #endif
GaspardD 11:bc24b3ba51a9 518 switch(st_maxSpeed)
GaspardD 11:bc24b3ba51a9 519 {
GaspardD 11:bc24b3ba51a9 520 case SPEED_LIMITED:
GaspardD 11:bc24b3ba51a9 521 maxSpeed_cmps = p_sectionCourante->slowSpeed_cmps;
GaspardD 11:bc24b3ba51a9 522 break;
GaspardD 11:bc24b3ba51a9 523 case SPEED_VARIABLE:
GaspardD 11:bc24b3ba51a9 524 maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps * (distLidar/(distLidar+p_sectionCourante->brakingCoefficient));
GaspardD 11:bc24b3ba51a9 525 break;
GaspardD 11:bc24b3ba51a9 526 default:
GaspardD 11:bc24b3ba51a9 527 maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps;
GaspardD 11:bc24b3ba51a9 528 break;
GaspardD 11:bc24b3ba51a9 529 }
GaspardD 8:1d8c3ca5e508 530 return;
GaspardD 8:1d8c3ca5e508 531 }
GaspardD 11:bc24b3ba51a9 532
GaspardD 11:bc24b3ba51a9 533 void throttleOutput(void)
GaspardD 11:bc24b3ba51a9 534 {
GaspardD 14:d471faa7d1a2 535 #if (DEBUG > 3)
GaspardD 14:d471faa7d1a2 536 pc.printf("\r\n Output TROTTLE\r\n");
GaspardD 14:d471faa7d1a2 537 #endif
GaspardD 11:bc24b3ba51a9 538 switch (st_thro) {
GaspardD 11:bc24b3ba51a9 539 case UNDER_SPEED:
GaspardD 17:8c465656eea4 540 if ( (pulseSpeed + p_sectionCourante->us_accel) < MAX_PULSE_WIDTH_FOR_TACHY)
GaspardD 15:129f205ff030 541 {
GaspardD 17:8c465656eea4 542 pulseSpeed += p_sectionCourante->us_accel;
GaspardD 15:129f205ff030 543 }
GaspardD 11:bc24b3ba51a9 544 break;
GaspardD 11:bc24b3ba51a9 545 case OVER_SPEED:
GaspardD 17:8c465656eea4 546 if ( (pulseSpeed - p_sectionCourante->us_decel) > INITAL_PULSE_SPEED)
GaspardD 15:129f205ff030 547 {
GaspardD 17:8c465656eea4 548 pulseSpeed -= p_sectionCourante->us_decel;
GaspardD 15:129f205ff030 549 }
GaspardD 11:bc24b3ba51a9 550 break;
GaspardD 11:bc24b3ba51a9 551 case AT_SPEED:
GaspardD 16:63690703b5b6 552 break;
GaspardD 16:63690703b5b6 553 case BRAKING:
GaspardD 16:63690703b5b6 554 #if DEBUG > 2
GaspardD 16:63690703b5b6 555 pc.printf("BRAKINGGGGGGGGGGGGGGGGGG !!! \r\n");
GaspardD 16:63690703b5b6 556 #endif
GaspardD 16:63690703b5b6 557 pulseSpeed = BRAKING_PWM;
GaspardD 16:63690703b5b6 558 break;
GaspardD 11:bc24b3ba51a9 559 default:
GaspardD 11:bc24b3ba51a9 560 break;
GaspardD 11:bc24b3ba51a9 561 }
GaspardD 17:8c465656eea4 562 PwmMotor.pulsewidth_us(pulseSpeed);
GaspardD 15:129f205ff030 563 #if DEBUG > 1
GaspardD 17:8c465656eea4 564 pc.printf("PWM Thro pulse: %d\r\n",pulseSpeed);
GaspardD 15:129f205ff030 565 #endif
GaspardD 14:d471faa7d1a2 566 #if DEBUG > 0
GaspardD 13:af9a59ccf60b 567 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 568 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 569 //wait(2);
GaspardD 13:af9a59ccf60b 570 timeSinceStart.reset();
GaspardD 13:af9a59ccf60b 571 timeSinceStart.start();
GaspardD 13:af9a59ccf60b 572 #endif
GaspardD 11:bc24b3ba51a9 573 return;
GaspardD 11:bc24b3ba51a9 574 }