![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TRR2018 omar
Fork of biniou by
stateMachines.cpp@25:3d03756a7fa5, 2018-09-16 (annotated)
- Committer:
- GaspardD
- Date:
- Sun Sep 16 17:09:26 2018 +0000
- Revision:
- 25:3d03756a7fa5
- Parent:
- 24:698fefbbee00
- Child:
- 26:3b144f243646
gestion en m/s correction des erreurs
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
GaspardD | 12:51b1b40cc017 | 1 | #include "stateMachines.h" |
GaspardD | 19:771bf61be276 | 2 | #if DEBUG >0 |
GaspardD | 13:af9a59ccf60b | 3 | Serial pc(USBTX, USBRX); // tx, rx |
GaspardD | 13:af9a59ccf60b | 4 | #endif |
GaspardD | 19:771bf61be276 | 5 | #if DEBUG >= -1 |
GaspardD | 19:771bf61be276 | 6 | InterruptIn my_button(USER_BUTTON); |
GaspardD | 23:04d393220daa | 7 | #ifdef DUMP_SAMPLIG_PERIOD |
GaspardD | 22:26fc6e6f7a55 | 8 | Timer timerLog; |
GaspardD | 19:771bf61be276 | 9 | #endif |
GaspardD | 23:04d393220daa | 10 | #endif |
GaspardD | 19:771bf61be276 | 11 | |
GaspardD | 11:bc24b3ba51a9 | 12 | uint16_t distMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne |
GaspardD | 11:bc24b3ba51a9 | 13 | uint16_t distMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit pour moyenne |
GaspardD | 11:bc24b3ba51a9 | 14 | uint16_t distMurG45[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 45deg pour moyenne |
GaspardD | 11:bc24b3ba51a9 | 15 | uint16_t distMurD45[NB_ECHANTILLONS_IR];//buffer tournant ir avant droit 45deg pour moyenne |
GaspardD | 11:bc24b3ba51a9 | 16 | #ifdef DLVV |
GaspardD | 11:bc24b3ba51a9 | 17 | uint16_t distMurG10[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 10deg pour moyenne |
GaspardD | 11:bc24b3ba51a9 | 18 | uint16_t distMurD10[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit 10deg pour moyenne |
GaspardD | 11:bc24b3ba51a9 | 19 | uint16_t distMurFront[NB_ECHANTILLONS_IR];//buffer tournant ir front |
GaspardD | 11:bc24b3ba51a9 | 20 | #endif |
GaspardD | 12:51b1b40cc017 | 21 | uint16_t distMurG90Moy; |
GaspardD | 12:51b1b40cc017 | 22 | uint16_t distMurD90Moy; |
GaspardD | 12:51b1b40cc017 | 23 | uint16_t distMurG45Moy; |
GaspardD | 12:51b1b40cc017 | 24 | uint16_t distMurD45Moy; |
GaspardD | 23:04d393220daa | 25 | |
GaspardD | 12:51b1b40cc017 | 26 | #ifdef DLVV |
GaspardD | 12:51b1b40cc017 | 27 | uint16_t distMurG10Moy; |
GaspardD | 12:51b1b40cc017 | 28 | uint16_t distMurD10Moy; |
GaspardD | 12:51b1b40cc017 | 29 | uint16_t distMurFrontMoy |
GaspardD | 12:51b1b40cc017 | 30 | #endif |
GaspardD | 8:1d8c3ca5e508 | 31 | int index_fifo_ir = 0;//pour géreer le buffer tournant |
GaspardD | 8:1d8c3ca5e508 | 32 | int index_fifo_lidar = 0; |
GaspardD | 11:bc24b3ba51a9 | 33 | //sections |
GaspardD | 11:bc24b3ba51a9 | 34 | s_Section p_section1; |
GaspardD | 23:04d393220daa | 35 | s_Section p_section2; |
GaspardD | 2:fd0ffe46a87d | 36 | |
GaspardD | 15:129f205ff030 | 37 | //PWM Controls |
GaspardD | 12:51b1b40cc017 | 38 | PwmOut PwmMotor(PB_6); // PWM4 ch1 TIM4 |
GaspardD | 12:51b1b40cc017 | 39 | PwmOut PwmDirection(PB_5); // PWM3 ch2 TIM3 |
GaspardD | 8:1d8c3ca5e508 | 40 | |
GaspardD | 21:de7a0a47f8a3 | 41 | int pulseDirection_us = DIRECTION_PULSE_MIDDLE; |
GaspardD | 19:771bf61be276 | 42 | int pulseSpeed_us = INITAL_PULSE_SPEED_US; |
GaspardD | 12:51b1b40cc017 | 43 | //Capteurs direction |
GaspardD | 8:1d8c3ca5e508 | 44 | |
GaspardD | 22:26fc6e6f7a55 | 45 | |
GaspardD | 22:26fc6e6f7a55 | 46 | |
GaspardD | 22:26fc6e6f7a55 | 47 | AnalogIn anaG90(CAPT_90_GAUCHE);//capteur ir coté gauche |
GaspardD | 22:26fc6e6f7a55 | 48 | AnalogIn anaD90(CAPT_90_DROITE);//capteur ir coté droit |
GaspardD | 22:26fc6e6f7a55 | 49 | AnalogIn anaG45(CAPT_45_GAUCHE);//capteur ir avant gauche 45 deg |
GaspardD | 22:26fc6e6f7a55 | 50 | AnalogIn anaD45(CAPT_45_DROITE);//capteur ir avant droit 45deg |
GaspardD | 11:bc24b3ba51a9 | 51 | #ifdef DLVV |
GaspardD | 22:26fc6e6f7a55 | 52 | AnalogIn anaDlvvG(CAPT_10_GAUCHE);//capteur ir avant droit 10 deg |
GaspardD | 22:26fc6e6f7a55 | 53 | AnalogIn anaDlvvD(CAPT_10_DROITE);//capteur ir coté droit 10 deg |
GaspardD | 22:26fc6e6f7a55 | 54 | AnalogIn anaDlvvFront(CAPT_DEVANT);//capteur ir avant |
GaspardD | 11:bc24b3ba51a9 | 55 | #endif |
GaspardD | 12:51b1b40cc017 | 56 | |
GaspardD | 19:771bf61be276 | 57 | int differenceGD45,differenceGD90,prevDiffGD90,prevDiffGD45,derive45,derive90; |
GaspardD | 21:de7a0a47f8a3 | 58 | int lastDifferences90[NB_INTEGRAL_SAMPLES] = {0};//for integral correction |
GaspardD | 21:de7a0a47f8a3 | 59 | int lastDifferenceIndex = 0; |
GaspardD | 21:de7a0a47f8a3 | 60 | int integralSum; |
GaspardD | 12:51b1b40cc017 | 61 | |
GaspardD | 12:51b1b40cc017 | 62 | //Capteur vitesse |
GaspardD | 12:51b1b40cc017 | 63 | InterruptIn it_tachymeter(PA_11); |
GaspardD | 2:fd0ffe46a87d | 64 | |
GaspardD | 11:bc24b3ba51a9 | 65 | |
GaspardD | 11:bc24b3ba51a9 | 66 | //LIDAR |
GaspardD | 12:51b1b40cc017 | 67 | Serial serialLidar(PC_10,PC_11); // tx, rx |
GaspardD | 11:bc24b3ba51a9 | 68 | |
GaspardD | 11:bc24b3ba51a9 | 69 | int distLidar;// LiDAR actually measured distance value |
GaspardD | 23:04d393220daa | 70 | int distLidarPrev; |
GaspardD | 11:bc24b3ba51a9 | 71 | int strengthLidar;// LiDAR signal strength |
GaspardD | 23:04d393220daa | 72 | int strengthLidarPrev; |
GaspardD | 11:bc24b3ba51a9 | 73 | int check;// check numerical value storage |
GaspardD | 12:51b1b40cc017 | 74 | int i; |
GaspardD | 11:bc24b3ba51a9 | 75 | int uart[9];// store data measured by LiDAR |
GaspardD | 11:bc24b3ba51a9 | 76 | const int HEADER=0x59;// data package frame header |
GaspardD | 11:bc24b3ba51a9 | 77 | |
GaspardD | 11:bc24b3ba51a9 | 78 | //SPEED |
GaspardD | 11:bc24b3ba51a9 | 79 | int maxSpeed_cmps = 0; |
GaspardD | 11:bc24b3ba51a9 | 80 | int tachySpeed_cmps = 0; |
GaspardD | 11:bc24b3ba51a9 | 81 | int tachyStepsRegister = 0; |
GaspardD | 11:bc24b3ba51a9 | 82 | int tachySectionDist_cm = 0; |
GaspardD | 8:1d8c3ca5e508 | 83 | |
GaspardD | 24:698fefbbee00 | 84 | Timer brakingTimer; |
GaspardD | 24:698fefbbee00 | 85 | int brakingDurationNeeded_us = 0; |
GaspardD | 24:698fefbbee00 | 86 | |
GaspardD | 8:1d8c3ca5e508 | 87 | //Etats |
GaspardD | 8:1d8c3ca5e508 | 88 | MUR_ST st_murs; |
GaspardD | 21:de7a0a47f8a3 | 89 | MUR_ST st_tmpMurs; |
GaspardD | 11:bc24b3ba51a9 | 90 | SECTION_ST st_currentSection; |
GaspardD | 21:de7a0a47f8a3 | 91 | SECTION_ST st_tmpSection; |
GaspardD | 11:bc24b3ba51a9 | 92 | MAX_SPEED_ST st_maxSpeed; |
GaspardD | 21:de7a0a47f8a3 | 93 | MAX_SPEED_ST st_tmpMaxSpeed; |
GaspardD | 8:1d8c3ca5e508 | 94 | THROTTLE_ST st_thro; |
GaspardD | 21:de7a0a47f8a3 | 95 | THROTTLE_ST st_tmpThro; |
GaspardD | 11:bc24b3ba51a9 | 96 | #ifdef DLVV |
GaspardD | 11:bc24b3ba51a9 | 97 | OBSTACLE_ST st_obstacle; |
GaspardD | 11:bc24b3ba51a9 | 98 | #endif |
GaspardD | 11:bc24b3ba51a9 | 99 | |
GaspardD | 11:bc24b3ba51a9 | 100 | s_Section* p_sectionCourante = NULL; |
GaspardD | 8:1d8c3ca5e508 | 101 | |
GaspardD | 8:1d8c3ca5e508 | 102 | //time monitoring |
GaspardD | 8:1d8c3ca5e508 | 103 | |
GaspardD | 11:bc24b3ba51a9 | 104 | Timer timeSinceStart;// temps.start()/stop()/sec: read()/ms: read_ms()/µs: read_us() |
GaspardD | 11:bc24b3ba51a9 | 105 | Timer timerSinceGetTachy; |
GaspardD | 11:bc24b3ba51a9 | 106 | |
GaspardD | 19:771bf61be276 | 107 | |
GaspardD | 19:771bf61be276 | 108 | // +++++++++++++++++++++++++++++++++++++++++++ SAMPLING +++++++++++++++++++++++++++++++++++++++++++ |
GaspardD | 19:771bf61be276 | 109 | #ifdef SAMPLING |
GaspardD | 19:771bf61be276 | 110 | s_Sample history[TAILLE_SAMPLES]; |
GaspardD | 19:771bf61be276 | 111 | int indexSample = 0; |
GaspardD | 19:771bf61be276 | 112 | void initSamples(void) |
GaspardD | 19:771bf61be276 | 113 | { |
GaspardD | 19:771bf61be276 | 114 | for(int m=0; m<TAILLE_SAMPLES; m++) { |
GaspardD | 19:771bf61be276 | 115 | history[m].states.murs_dlvv = '0'; |
GaspardD | 19:771bf61be276 | 116 | history[m].states.section = '0'; |
GaspardD | 19:771bf61be276 | 117 | history[m].states.maxSpeed = '0'; |
GaspardD | 19:771bf61be276 | 118 | history[m].states.throttle = '0'; |
GaspardD | 19:771bf61be276 | 119 | history[m].time = 0; |
GaspardD | 21:de7a0a47f8a3 | 120 | history[m].diffgd45 = 0; |
GaspardD | 21:de7a0a47f8a3 | 121 | history[m].diffgd90 = 0; |
GaspardD | 21:de7a0a47f8a3 | 122 | history[m].pwm_thro_us = 0; |
GaspardD | 21:de7a0a47f8a3 | 123 | history[m].pwm_dir_us = 0; |
GaspardD | 23:04d393220daa | 124 | history[m].distLidar = 0; |
GaspardD | 23:04d393220daa | 125 | history[m].strLidar = 0; |
GaspardD | 19:771bf61be276 | 126 | } |
GaspardD | 21:de7a0a47f8a3 | 127 | #if DEBUG > 0 |
GaspardD | 19:771bf61be276 | 128 | pc.printf("[INIT SAMPLE DONE]\r\n"); |
GaspardD | 21:de7a0a47f8a3 | 129 | #endif |
GaspardD | 19:771bf61be276 | 130 | return; |
GaspardD | 19:771bf61be276 | 131 | } |
GaspardD | 19:771bf61be276 | 132 | void sampleLog(void) |
GaspardD | 19:771bf61be276 | 133 | { |
GaspardD | 23:04d393220daa | 134 | #ifdef DUMP_SAMPLIG_PERIOD |
GaspardD | 23:04d393220daa | 135 | if(timerLog.read_us() > DUMP_SAMPLIG_PERIOD) { |
GaspardD | 22:26fc6e6f7a55 | 136 | timerLog.reset(); |
GaspardD | 22:26fc6e6f7a55 | 137 | timerLog.start(); |
GaspardD | 23:04d393220daa | 138 | #endif |
GaspardD | 22:26fc6e6f7a55 | 139 | if(indexSample < TAILLE_SAMPLES) { |
GaspardD | 19:771bf61be276 | 140 | #ifdef DLVV |
GaspardD | 22:26fc6e6f7a55 | 141 | history[indexSample].states.murs_dlvv = (char)st_obstacle; |
GaspardD | 19:771bf61be276 | 142 | #else |
GaspardD | 22:26fc6e6f7a55 | 143 | history[indexSample].states.murs_dlvv = (char)st_murs; |
GaspardD | 19:771bf61be276 | 144 | #endif |
GaspardD | 22:26fc6e6f7a55 | 145 | history[indexSample].states.section = (char)st_currentSection; |
GaspardD | 22:26fc6e6f7a55 | 146 | history[indexSample].states.maxSpeed = (char)st_maxSpeed; |
GaspardD | 22:26fc6e6f7a55 | 147 | history[indexSample].states.throttle = (char)st_thro; |
GaspardD | 22:26fc6e6f7a55 | 148 | history[indexSample].time = timeSinceStart.read_us() ; |
GaspardD | 22:26fc6e6f7a55 | 149 | history[indexSample].diffgd45 = differenceGD45; |
GaspardD | 22:26fc6e6f7a55 | 150 | history[indexSample].diffgd90 = differenceGD90; |
GaspardD | 22:26fc6e6f7a55 | 151 | history[indexSample].pwm_thro_us = pulseSpeed_us; |
GaspardD | 22:26fc6e6f7a55 | 152 | history[indexSample].pwm_dir_us = pulseDirection_us; |
GaspardD | 22:26fc6e6f7a55 | 153 | history[indexSample].dist = tachySectionDist_cm, |
GaspardD | 23:04d393220daa | 154 | history[indexSample].distLidar = distLidar; |
GaspardD | 23:04d393220daa | 155 | history[indexSample].strLidar = strengthLidar; |
GaspardD | 23:04d393220daa | 156 | indexSample++; |
GaspardD | 19:771bf61be276 | 157 | #if DEBUG > 0 |
GaspardD | 22:26fc6e6f7a55 | 158 | 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 | 22:26fc6e6f7a55 | 159 | 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 | 22:26fc6e6f7a55 | 160 | //wait(2); |
GaspardD | 22:26fc6e6f7a55 | 161 | timeSinceStart.reset(); |
GaspardD | 22:26fc6e6f7a55 | 162 | timeSinceStart.start(); |
GaspardD | 19:771bf61be276 | 163 | #endif |
GaspardD | 22:26fc6e6f7a55 | 164 | } |
GaspardD | 22:26fc6e6f7a55 | 165 | return; |
GaspardD | 23:04d393220daa | 166 | #ifdef DUMP_SAMPLIG_PERIOD |
GaspardD | 19:771bf61be276 | 167 | } |
GaspardD | 23:04d393220daa | 168 | #endif |
GaspardD | 19:771bf61be276 | 169 | } |
GaspardD | 19:771bf61be276 | 170 | void transmitData(void) |
GaspardD | 19:771bf61be276 | 171 | { |
GaspardD | 21:de7a0a47f8a3 | 172 | #if DEBUG > 0 |
GaspardD | 19:771bf61be276 | 173 | pc.printf("[START TO TRANSMIT]\r\n"); |
GaspardD | 21:de7a0a47f8a3 | 174 | #endif |
GaspardD | 22:26fc6e6f7a55 | 175 | serialLidar.printf("time,diffgd 45,diffgd 90,pwm_thro_us,pwm_dir_us,dist,murs/dlvv,section,maxSpeed,throttle\r\n"); |
GaspardD | 19:771bf61be276 | 176 | for(int p=0; p<indexSample; p++) { |
GaspardD | 22:26fc6e6f7a55 | 177 | serialLidar.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n", |
GaspardD | 21:de7a0a47f8a3 | 178 | history[p].time, |
GaspardD | 21:de7a0a47f8a3 | 179 | history[p].diffgd45, |
GaspardD | 21:de7a0a47f8a3 | 180 | history[p].diffgd90, |
GaspardD | 21:de7a0a47f8a3 | 181 | history[p].pwm_thro_us, |
GaspardD | 21:de7a0a47f8a3 | 182 | history[p].pwm_dir_us, |
GaspardD | 21:de7a0a47f8a3 | 183 | history[p].dist, |
GaspardD | 21:de7a0a47f8a3 | 184 | history[p].states.murs_dlvv, |
GaspardD | 21:de7a0a47f8a3 | 185 | history[p].states.section, |
GaspardD | 21:de7a0a47f8a3 | 186 | history[p].states.maxSpeed, |
GaspardD | 21:de7a0a47f8a3 | 187 | history[p].states.throttle); |
GaspardD | 19:771bf61be276 | 188 | } |
GaspardD | 19:771bf61be276 | 189 | return; |
GaspardD | 19:771bf61be276 | 190 | } |
GaspardD | 19:771bf61be276 | 191 | |
GaspardD | 19:771bf61be276 | 192 | #endif |
GaspardD | 19:771bf61be276 | 193 | |
GaspardD | 19:771bf61be276 | 194 | // +++++++++++++++++++++++++++++++++++++++++++ FUNCTION UTILS +++++++++++++++++++++++++++++++++++++++++++ |
GaspardD | 19:771bf61be276 | 195 | |
GaspardD | 19:771bf61be276 | 196 | #if DEBUG >= -1 |
GaspardD | 19:771bf61be276 | 197 | void pressed(void) |
GaspardD | 19:771bf61be276 | 198 | { |
GaspardD | 21:de7a0a47f8a3 | 199 | #if DEBUG > 0 |
GaspardD | 19:771bf61be276 | 200 | pc.printf("[BTN PRESSED]\r\n"); |
GaspardD | 21:de7a0a47f8a3 | 201 | #endif |
GaspardD | 19:771bf61be276 | 202 | //p_sectionCourante = &p_section1; |
GaspardD | 19:771bf61be276 | 203 | transmitData(); |
GaspardD | 19:771bf61be276 | 204 | } |
GaspardD | 19:771bf61be276 | 205 | #endif |
GaspardD | 19:771bf61be276 | 206 | |
GaspardD | 11:bc24b3ba51a9 | 207 | void getTachySpeed() |
GaspardD | 11:bc24b3ba51a9 | 208 | { |
GaspardD | 11:bc24b3ba51a9 | 209 | //tachySteps = VALEUR_DU_PIN; |
GaspardD | 12:51b1b40cc017 | 210 | //poour gérer les vitesses lentes |
GaspardD | 19:771bf61be276 | 211 | if(tachyStepsRegister == 0 && timerSinceGetTachy.read_us() < 500000) { |
GaspardD | 12:51b1b40cc017 | 212 | return;//on attend encore un peu l'aquisition de la vitesse |
GaspardD | 12:51b1b40cc017 | 213 | } |
GaspardD | 15:129f205ff030 | 214 | tachySectionDist_cm += tachyStepsRegister; |
GaspardD | 15:129f205ff030 | 215 | tachySpeed_cmps = (tachyStepsRegister * 1000000)/timerSinceGetTachy.read_us(); |
GaspardD | 19:771bf61be276 | 216 | #if DEBUG > 2 |
GaspardD | 19:771bf61be276 | 217 | pc.printf("IT: distance parcourue %d , vitesse:%d \r\n",tachySectionDist_cm,tachySpeed_cmps); |
GaspardD | 19:771bf61be276 | 218 | #endif |
GaspardD | 11:bc24b3ba51a9 | 219 | tachyStepsRegister=0; |
GaspardD | 11:bc24b3ba51a9 | 220 | timerSinceGetTachy.reset(); |
GaspardD | 11:bc24b3ba51a9 | 221 | timerSinceGetTachy.start(); |
GaspardD | 11:bc24b3ba51a9 | 222 | return; |
GaspardD | 11:bc24b3ba51a9 | 223 | } |
GaspardD | 8:1d8c3ca5e508 | 224 | |
GaspardD | 21:de7a0a47f8a3 | 225 | |
GaspardD | 12:51b1b40cc017 | 226 | uint16_t getDistMoy(uint16_t* tab,int size) |
GaspardD | 11:bc24b3ba51a9 | 227 | { |
GaspardD | 11:bc24b3ba51a9 | 228 | int sumMoy = 0; |
GaspardD | 19:771bf61be276 | 229 | for(int k=0; k<size; k++) { |
GaspardD | 11:bc24b3ba51a9 | 230 | sumMoy+=tab[k]; |
GaspardD | 11:bc24b3ba51a9 | 231 | } |
GaspardD | 11:bc24b3ba51a9 | 232 | return sumMoy/size; |
GaspardD | 11:bc24b3ba51a9 | 233 | } |
GaspardD | 2:fd0ffe46a87d | 234 | |
GaspardD | 12:51b1b40cc017 | 235 | void it4cm() |
GaspardD | 12:51b1b40cc017 | 236 | { |
GaspardD | 22:26fc6e6f7a55 | 237 | tachyStepsRegister+=TACHY_CM; |
GaspardD | 19:771bf61be276 | 238 | #if DEBUG > 0 |
GaspardD | 14:d471faa7d1a2 | 239 | pc.printf("IT tachy\r\n"); |
GaspardD | 14:d471faa7d1a2 | 240 | #endif |
GaspardD | 12:51b1b40cc017 | 241 | } |
GaspardD | 12:51b1b40cc017 | 242 | |
GaspardD | 12:51b1b40cc017 | 243 | void it_serial() |
GaspardD | 12:51b1b40cc017 | 244 | { |
GaspardD | 12:51b1b40cc017 | 245 | if(serialLidar.getc()==HEADER) { // determine data package frame header 0x59 |
GaspardD | 12:51b1b40cc017 | 246 | uart[0]=HEADER; |
GaspardD | 12:51b1b40cc017 | 247 | if(serialLidar.getc()==HEADER) { //determine data package frame header 0x59 |
GaspardD | 12:51b1b40cc017 | 248 | uart[1]=HEADER; |
GaspardD | 12:51b1b40cc017 | 249 | for(i=2; i<9; i++) { // store data to array |
GaspardD | 12:51b1b40cc017 | 250 | uart[i]=serialLidar.getc(); |
GaspardD | 12:51b1b40cc017 | 251 | } |
GaspardD | 12:51b1b40cc017 | 252 | check=uart[0]+uart[1]+uart[2]+uart[3]+uart[4]+uart[5]+uart[6]+uart[7]; |
GaspardD | 12:51b1b40cc017 | 253 | if(uart[8]==(check&0xff)) { // check the received data as per protocols |
GaspardD | 23:04d393220daa | 254 | distLidarPrev = distLidar; |
GaspardD | 12:51b1b40cc017 | 255 | distLidar=uart[2]+uart[3]*256;// calculate distance value |
GaspardD | 23:04d393220daa | 256 | strengthLidarPrev = strengthLidar; |
GaspardD | 12:51b1b40cc017 | 257 | strengthLidar=uart[4]+uart[5]*256;// calculate signal strength value |
GaspardD | 12:51b1b40cc017 | 258 | } |
GaspardD | 12:51b1b40cc017 | 259 | } |
GaspardD | 12:51b1b40cc017 | 260 | } |
GaspardD | 12:51b1b40cc017 | 261 | } |
GaspardD | 19:771bf61be276 | 262 | // +++++++++++++++++++++++++++++++++++++++++++ STATES MACHINES +++++++++++++++++++++++++++++++++++++++++++ |
GaspardD | 19:771bf61be276 | 263 | |
GaspardD | 8:1d8c3ca5e508 | 264 | //########## INIT STATES MACHINES ########## |
GaspardD | 11:bc24b3ba51a9 | 265 | void mursInit(void) |
GaspardD | 8:1d8c3ca5e508 | 266 | { |
GaspardD | 19:771bf61be276 | 267 | #if DEBUG >= -1 |
GaspardD | 19:771bf61be276 | 268 | my_button.fall(&pressed); |
GaspardD | 19:771bf61be276 | 269 | initSamples(); |
GaspardD | 19:771bf61be276 | 270 | #endif |
GaspardD | 19:771bf61be276 | 271 | #if DEBUG > 0 |
GaspardD | 13:af9a59ccf60b | 272 | pc.baud(115200); |
GaspardD | 13:af9a59ccf60b | 273 | pc.printf("Init Murs\r\n"); |
GaspardD | 13:af9a59ccf60b | 274 | #endif |
GaspardD | 13:af9a59ccf60b | 275 | timeSinceStart.start(); |
GaspardD | 11:bc24b3ba51a9 | 276 | st_murs=EQUILIBRAGE_REPULSIF; |
GaspardD | 19:771bf61be276 | 277 | PwmDirection.period_us(SPEED_PERIOD_US); |
GaspardD | 17:8c465656eea4 | 278 | PwmDirection.pulsewidth_us(DIRECTION_PULSE_MIDDLE);// milieu |
GaspardD | 19:771bf61be276 | 279 | prevDiffGD45 = 0; |
GaspardD | 19:771bf61be276 | 280 | prevDiffGD90 = 0; |
GaspardD | 8:1d8c3ca5e508 | 281 | return; |
GaspardD | 8:1d8c3ca5e508 | 282 | } |
GaspardD | 11:bc24b3ba51a9 | 283 | #ifdef DLVV |
GaspardD | 11:bc24b3ba51a9 | 284 | void obstacleInit(void) |
GaspardD | 8:1d8c3ca5e508 | 285 | { |
GaspardD | 19:771bf61be276 | 286 | #if DEBUG > 0 |
GaspardD | 13:af9a59ccf60b | 287 | pc.printf("Init Obstacle\r\n"); |
GaspardD | 13:af9a59ccf60b | 288 | #endif |
GaspardD | 11:bc24b3ba51a9 | 289 | st_speedLimit=FRONT_CLEAR; |
GaspardD | 8:1d8c3ca5e508 | 290 | return; |
GaspardD | 8:1d8c3ca5e508 | 291 | } |
GaspardD | 11:bc24b3ba51a9 | 292 | #endif |
GaspardD | 11:bc24b3ba51a9 | 293 | void sectionInit(void) |
GaspardD | 2:fd0ffe46a87d | 294 | { |
GaspardD | 19:771bf61be276 | 295 | #if DEBUG > 0 |
GaspardD | 13:af9a59ccf60b | 296 | pc.printf("Init Section\r\n"); |
GaspardD | 13:af9a59ccf60b | 297 | #endif |
GaspardD | 11:bc24b3ba51a9 | 298 | st_currentSection=ARRET; |
GaspardD | 14:d471faa7d1a2 | 299 | p_sectionCourante=&p_section1; |
GaspardD | 12:51b1b40cc017 | 300 | it_tachymeter.fall(&it4cm); |
GaspardD | 11:bc24b3ba51a9 | 301 | timerSinceGetTachy.start(); |
GaspardD | 11:bc24b3ba51a9 | 302 | getTachySpeed();//to reset |
GaspardD | 11:bc24b3ba51a9 | 303 | tachySectionDist_cm = 0; |
GaspardD | 11:bc24b3ba51a9 | 304 | tachyStepsRegister = 0; |
GaspardD | 11:bc24b3ba51a9 | 305 | |
GaspardD | 23:04d393220daa | 306 | //section de test 1 |
GaspardD | 23:04d393220daa | 307 | p_section1.nextSection =NULL;// &p_section2; |
GaspardD | 23:04d393220daa | 308 | p_section1.targetSpeed_cmps = 400; |
GaspardD | 22:26fc6e6f7a55 | 309 | p_section1.slowSpeed_cmps = 328; |
GaspardD | 23:04d393220daa | 310 | p_section1.brakingCoefficient = 61; // application de la formule |
GaspardD | 21:de7a0a47f8a3 | 311 | p_section1.coef_p_speed = 1; |
GaspardD | 23:04d393220daa | 312 | p_section1.lidarWarningDist_cm = 120; |
GaspardD | 23:04d393220daa | 313 | p_section1.lng_section_cm = 30000;//30m |
GaspardD | 23:04d393220daa | 314 | p_section1.coef_p = 50;//35; |
GaspardD | 23:04d393220daa | 315 | p_section1.coef_i = 10000;//35*NB_INTEGRAL_SAMPLES; |
GaspardD | 23:04d393220daa | 316 | p_section1.coef_d = 10000;//35; |
GaspardD | 23:04d393220daa | 317 | |
GaspardD | 23:04d393220daa | 318 | //section de test |
GaspardD | 23:04d393220daa | 319 | p_section2.nextSection = NULL; |
GaspardD | 23:04d393220daa | 320 | p_section2.targetSpeed_cmps = 328; |
GaspardD | 23:04d393220daa | 321 | p_section2.slowSpeed_cmps = 328; |
GaspardD | 23:04d393220daa | 322 | p_section2.brakingCoefficient = 0; // application de la formule |
GaspardD | 23:04d393220daa | 323 | p_section2.coef_p_speed = 1; |
GaspardD | 23:04d393220daa | 324 | p_section2.lidarWarningDist_cm = 300; |
GaspardD | 23:04d393220daa | 325 | p_section2.lng_section_cm = 200;//2m |
GaspardD | 23:04d393220daa | 326 | p_section2.coef_p = 50; |
GaspardD | 23:04d393220daa | 327 | p_section2.coef_i = 10000;//35*NB_INTEGRAL_SAMPLES; |
GaspardD | 23:04d393220daa | 328 | p_section2.coef_d = 10000;//35; |
GaspardD | 23:04d393220daa | 329 | |
GaspardD | 8:1d8c3ca5e508 | 330 | return; |
GaspardD | 8:1d8c3ca5e508 | 331 | } |
GaspardD | 11:bc24b3ba51a9 | 332 | |
GaspardD | 11:bc24b3ba51a9 | 333 | void maxSpeedInit(void) |
GaspardD | 11:bc24b3ba51a9 | 334 | { |
GaspardD | 19:771bf61be276 | 335 | #if DEBUG > 0 |
GaspardD | 13:af9a59ccf60b | 336 | pc.printf("Init Max Speed\r\n"); |
GaspardD | 13:af9a59ccf60b | 337 | #endif |
GaspardD | 11:bc24b3ba51a9 | 338 | st_maxSpeed=SPEED_MAX; |
GaspardD | 11:bc24b3ba51a9 | 339 | maxSpeed_cmps= p_sectionCourante->targetSpeed_cmps; |
GaspardD | 11:bc24b3ba51a9 | 340 | serialLidar.baud(115200); |
GaspardD | 14:d471faa7d1a2 | 341 | serialLidar.attach(&it_serial); |
GaspardD | 11:bc24b3ba51a9 | 342 | return; |
GaspardD | 11:bc24b3ba51a9 | 343 | } |
GaspardD | 11:bc24b3ba51a9 | 344 | |
GaspardD | 11:bc24b3ba51a9 | 345 | void throttleInit(void) |
GaspardD | 8:1d8c3ca5e508 | 346 | { |
GaspardD | 19:771bf61be276 | 347 | #if DEBUG > 0 |
GaspardD | 13:af9a59ccf60b | 348 | pc.printf("Init Throttle\r\n"); |
GaspardD | 13:af9a59ccf60b | 349 | #endif |
GaspardD | 21:de7a0a47f8a3 | 350 | st_thro = REGULATION_SPEED; |
GaspardD | 19:771bf61be276 | 351 | PwmMotor.period_us(DIERCTION_PERIOD_MS); //20 ms is default |
GaspardD | 19:771bf61be276 | 352 | PwmMotor.pulsewidth_us(1000);//MIN |
GaspardD | 12:51b1b40cc017 | 353 | wait(3); |
GaspardD | 19:771bf61be276 | 354 | PwmMotor.pulsewidth_us(2000);//MAX |
GaspardD | 12:51b1b40cc017 | 355 | wait(1); |
GaspardD | 19:771bf61be276 | 356 | PwmMotor.pulsewidth_us(1500);//ZEROING |
GaspardD | 12:51b1b40cc017 | 357 | wait(1); |
GaspardD | 19:771bf61be276 | 358 | pulseSpeed_us = INITAL_PULSE_SPEED_US; |
GaspardD | 19:771bf61be276 | 359 | #if DEBUG > 0 |
GaspardD | 13:af9a59ccf60b | 360 | pc.printf("temps init: %d micros\r\n",timeSinceStart.read_us()); |
GaspardD | 13:af9a59ccf60b | 361 | 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 | 362 | timeSinceStart.reset(); |
GaspardD | 13:af9a59ccf60b | 363 | timeSinceStart.start(); |
GaspardD | 13:af9a59ccf60b | 364 | #endif |
GaspardD | 23:04d393220daa | 365 | #ifdef DUMP_SAMPLIG_PERIOD |
GaspardD | 22:26fc6e6f7a55 | 366 | timerLog.start(); |
GaspardD | 22:26fc6e6f7a55 | 367 | #endif |
GaspardD | 22:26fc6e6f7a55 | 368 | |
GaspardD | 8:1d8c3ca5e508 | 369 | return; |
GaspardD | 8:1d8c3ca5e508 | 370 | } |
GaspardD | 2:fd0ffe46a87d | 371 | |
GaspardD | 8:1d8c3ca5e508 | 372 | |
GaspardD | 8:1d8c3ca5e508 | 373 | //########## UPDATE STATES ########## |
GaspardD | 11:bc24b3ba51a9 | 374 | void mursUpdate(void) |
GaspardD | 2:fd0ffe46a87d | 375 | { |
GaspardD | 14:d471faa7d1a2 | 376 | #if (DEBUG > 3) |
GaspardD | 14:d471faa7d1a2 | 377 | pc.printf("\r\nUpdate MURS\r\n"); |
GaspardD | 14:d471faa7d1a2 | 378 | #endif |
GaspardD | 11:bc24b3ba51a9 | 379 | //lectures |
GaspardD | 11:bc24b3ba51a9 | 380 | distMurG90[index_fifo_ir]=anaG90.read_u16(); |
GaspardD | 11:bc24b3ba51a9 | 381 | distMurD90[index_fifo_ir]=anaD90.read_u16(); |
GaspardD | 11:bc24b3ba51a9 | 382 | distMurG45[index_fifo_ir]=anaG45.read_u16(); |
GaspardD | 11:bc24b3ba51a9 | 383 | distMurD45[index_fifo_ir]=anaD45.read_u16(); |
GaspardD | 11:bc24b3ba51a9 | 384 | index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR; |
GaspardD | 23:04d393220daa | 385 | |
GaspardD | 12:51b1b40cc017 | 386 | distMurG45Moy = getDistMoy(distMurG45,NB_ECHANTILLONS_IR); |
GaspardD | 12:51b1b40cc017 | 387 | distMurD45Moy = getDistMoy(distMurD45,NB_ECHANTILLONS_IR); |
GaspardD | 23:04d393220daa | 388 | distMurG90Moy = getDistMoy(distMurG90,NB_ECHANTILLONS_IR); |
GaspardD | 23:04d393220daa | 389 | distMurD90Moy = getDistMoy(distMurD90,NB_ECHANTILLONS_IR); |
GaspardD | 12:51b1b40cc017 | 390 | |
GaspardD | 8:1d8c3ca5e508 | 391 | return; |
GaspardD | 8:1d8c3ca5e508 | 392 | } |
GaspardD | 11:bc24b3ba51a9 | 393 | #ifdef DLVV |
GaspardD | 11:bc24b3ba51a9 | 394 | void obstacleUpdate(void) |
GaspardD | 8:1d8c3ca5e508 | 395 | { |
GaspardD | 8:1d8c3ca5e508 | 396 | return; |
GaspardD | 8:1d8c3ca5e508 | 397 | } |
GaspardD | 11:bc24b3ba51a9 | 398 | #endif |
GaspardD | 11:bc24b3ba51a9 | 399 | |
GaspardD | 11:bc24b3ba51a9 | 400 | void sectionUpdate(void) |
GaspardD | 8:1d8c3ca5e508 | 401 | { |
GaspardD | 14:d471faa7d1a2 | 402 | #if (DEBUG > 3) |
GaspardD | 14:d471faa7d1a2 | 403 | pc.printf("\r\nUpdate Section\r\n"); |
GaspardD | 14:d471faa7d1a2 | 404 | #endif |
GaspardD | 11:bc24b3ba51a9 | 405 | switch (st_currentSection) { |
GaspardD | 19:771bf61be276 | 406 | case RUNNING_SECTION: |
GaspardD | 19:771bf61be276 | 407 | if(tachySectionDist_cm > p_sectionCourante->lng_section_cm) { //on pourrait rajouter un test lidar |
GaspardD | 19:771bf61be276 | 408 | st_tmpSection = LOADING_SECTION; |
GaspardD | 19:771bf61be276 | 409 | } else { |
GaspardD | 19:771bf61be276 | 410 | return; |
GaspardD | 19:771bf61be276 | 411 | } |
GaspardD | 19:771bf61be276 | 412 | break; |
GaspardD | 19:771bf61be276 | 413 | case LOADING_SECTION: |
GaspardD | 19:771bf61be276 | 414 | if(p_sectionCourante != NULL) { //la section a ete chargee dans sectionOutput |
GaspardD | 19:771bf61be276 | 415 | st_tmpSection = RUNNING_SECTION; |
GaspardD | 19:771bf61be276 | 416 | } else { |
GaspardD | 19:771bf61be276 | 417 | st_tmpSection=ARRET; |
GaspardD | 19:771bf61be276 | 418 | } |
GaspardD | 19:771bf61be276 | 419 | break; |
GaspardD | 19:771bf61be276 | 420 | case ARRET: |
GaspardD | 19:771bf61be276 | 421 | if(p_sectionCourante != NULL) { |
GaspardD | 19:771bf61be276 | 422 | st_tmpSection = RUNNING_SECTION; |
GaspardD | 19:771bf61be276 | 423 | } else { |
GaspardD | 19:771bf61be276 | 424 | return; |
GaspardD | 19:771bf61be276 | 425 | } |
GaspardD | 19:771bf61be276 | 426 | break; |
GaspardD | 19:771bf61be276 | 427 | default: |
GaspardD | 19:771bf61be276 | 428 | break; |
GaspardD | 11:bc24b3ba51a9 | 429 | } |
GaspardD | 11:bc24b3ba51a9 | 430 | st_currentSection = st_tmpSection; |
GaspardD | 8:1d8c3ca5e508 | 431 | return; |
GaspardD | 8:1d8c3ca5e508 | 432 | } |
GaspardD | 8:1d8c3ca5e508 | 433 | |
GaspardD | 11:bc24b3ba51a9 | 434 | void maxSpeedUpdate(void) |
GaspardD | 8:1d8c3ca5e508 | 435 | { |
GaspardD | 14:d471faa7d1a2 | 436 | #if (DEBUG > 3) |
GaspardD | 14:d471faa7d1a2 | 437 | pc.printf("\r\nUpdate MaxSpeed\r\n"); |
GaspardD | 14:d471faa7d1a2 | 438 | #endif |
GaspardD | 11:bc24b3ba51a9 | 439 | i=0; |
GaspardD | 11:bc24b3ba51a9 | 440 | |
GaspardD | 23:04d393220daa | 441 | if( strengthLidar > LIDAR_STRENGTH_THRESOLD ) { |
GaspardD | 23:04d393220daa | 442 | if( distLidar > p_sectionCourante->lidarWarningDist_cm ) { |
GaspardD | 19:771bf61be276 | 443 | st_tmpMaxSpeed = SPEED_MAX; |
GaspardD | 24:698fefbbee00 | 444 | } else if( strengthLidar > LIDAR_STRENGTH_THRESOLD && |
GaspardD | 24:698fefbbee00 | 445 | strengthLidarPrev > LIDAR_STRENGTH_THRESOLD && |
GaspardD | 24:698fefbbee00 | 446 | distLidar < p_sectionCourante->lidarWarningDist_cm && |
GaspardD | 24:698fefbbee00 | 447 | distLidarPrev-distLidar > 0) |
GaspardD | 24:698fefbbee00 | 448 | { |
GaspardD | 21:de7a0a47f8a3 | 449 | st_tmpMaxSpeed = BLOCKED; |
GaspardD | 11:bc24b3ba51a9 | 450 | } |
GaspardD | 24:698fefbbee00 | 451 | else{ |
GaspardD | 24:698fefbbee00 | 452 | st_tmpMaxSpeed = SPEED_WARNING; |
GaspardD | 24:698fefbbee00 | 453 | } |
GaspardD | 21:de7a0a47f8a3 | 454 | } else { |
GaspardD | 11:bc24b3ba51a9 | 455 | st_tmpMaxSpeed = SPEED_MAX; |
GaspardD | 11:bc24b3ba51a9 | 456 | } |
GaspardD | 11:bc24b3ba51a9 | 457 | |
GaspardD | 11:bc24b3ba51a9 | 458 | st_maxSpeed = st_tmpMaxSpeed; |
GaspardD | 11:bc24b3ba51a9 | 459 | return; |
GaspardD | 11:bc24b3ba51a9 | 460 | } |
GaspardD | 11:bc24b3ba51a9 | 461 | |
GaspardD | 11:bc24b3ba51a9 | 462 | void throttleUpdate(void) |
GaspardD | 11:bc24b3ba51a9 | 463 | { |
GaspardD | 14:d471faa7d1a2 | 464 | #if (DEBUG > 3) |
GaspardD | 14:d471faa7d1a2 | 465 | pc.printf("\r\nUpdate Throttle\r\n"); |
GaspardD | 14:d471faa7d1a2 | 466 | #endif |
GaspardD | 14:d471faa7d1a2 | 467 | getTachySpeed(); |
GaspardD | 11:bc24b3ba51a9 | 468 | switch (st_thro) { |
GaspardD | 21:de7a0a47f8a3 | 469 | case REGULATION_SPEED: |
GaspardD | 24:698fefbbee00 | 470 | if( st_currentSection == ARRET || |
GaspardD | 24:698fefbbee00 | 471 | st_maxSpeed == BLOCKED ) |
GaspardD | 24:698fefbbee00 | 472 | { |
GaspardD | 19:771bf61be276 | 473 | st_tmpThro = BRAKING; |
GaspardD | 24:698fefbbee00 | 474 | } else if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS)) |
GaspardD | 24:698fefbbee00 | 475 | { |
GaspardD | 24:698fefbbee00 | 476 | st_tmpThro = BRAKING; |
GaspardD | 25:3d03756a7fa5 | 477 | brakingDurationNeeded_us = (1000000)* MASSE_BINIOU_KG *((tachySpeed_cmps-maxSpeed_cmps)*(tachySpeed_cmps+maxSpeed_cmps)/100) / (PUISSANCE_FREINAGE_W *2) ; |
GaspardD | 24:698fefbbee00 | 478 | brakingTimer.reset(); |
GaspardD | 24:698fefbbee00 | 479 | brakingTimer.start(); |
GaspardD | 24:698fefbbee00 | 480 | } else |
GaspardD | 24:698fefbbee00 | 481 | { |
GaspardD | 19:771bf61be276 | 482 | return; |
GaspardD | 19:771bf61be276 | 483 | } |
GaspardD | 19:771bf61be276 | 484 | break; |
GaspardD | 16:63690703b5b6 | 485 | case BRAKING: |
GaspardD | 25:3d03756a7fa5 | 486 | if(brakingTimer.read_us() < brakingDurationNeeded_us || BLOCKED)//put a timer here |
GaspardD | 23:04d393220daa | 487 | { |
GaspardD | 23:04d393220daa | 488 | st_tmpThro = BRAKING; |
GaspardD | 23:04d393220daa | 489 | }else |
GaspardD | 21:de7a0a47f8a3 | 490 | if(st_currentSection == ARRET) { |
GaspardD | 21:de7a0a47f8a3 | 491 | st_tmpThro = STOPPED; |
GaspardD | 23:04d393220daa | 492 | } else{ |
GaspardD | 24:698fefbbee00 | 493 | brakingDurationNeeded_us = 0; |
GaspardD | 21:de7a0a47f8a3 | 494 | st_tmpThro = REGULATION_SPEED; |
GaspardD | 21:de7a0a47f8a3 | 495 | } |
GaspardD | 19:771bf61be276 | 496 | break; |
GaspardD | 19:771bf61be276 | 497 | case STOPPED: |
GaspardD | 19:771bf61be276 | 498 | if(st_currentSection == RUNNING_SECTION) { |
GaspardD | 22:26fc6e6f7a55 | 499 | st_tmpThro = REGULATION_SPEED; |
GaspardD | 19:771bf61be276 | 500 | } else { |
GaspardD | 19:771bf61be276 | 501 | st_tmpThro = STOPPED; |
GaspardD | 16:63690703b5b6 | 502 | } |
GaspardD | 19:771bf61be276 | 503 | break; |
GaspardD | 19:771bf61be276 | 504 | default: |
GaspardD | 19:771bf61be276 | 505 | break; |
GaspardD | 11:bc24b3ba51a9 | 506 | } |
GaspardD | 11:bc24b3ba51a9 | 507 | st_thro = st_tmpThro; |
GaspardD | 8:1d8c3ca5e508 | 508 | return; |
GaspardD | 8:1d8c3ca5e508 | 509 | } |
GaspardD | 2:fd0ffe46a87d | 510 | |
GaspardD | 8:1d8c3ca5e508 | 511 | //########## OUTPUT STATES ########## |
GaspardD | 11:bc24b3ba51a9 | 512 | //updating output parameters |
GaspardD | 11:bc24b3ba51a9 | 513 | void mursOutput(void) |
GaspardD | 12:51b1b40cc017 | 514 | { |
GaspardD | 21:de7a0a47f8a3 | 515 | prevDiffGD90 = differenceGD90; |
GaspardD | 21:de7a0a47f8a3 | 516 | prevDiffGD45 = differenceGD45; |
GaspardD | 14:d471faa7d1a2 | 517 | #if (DEBUG > 3) |
GaspardD | 14:d471faa7d1a2 | 518 | pc.printf("\r\n Output MURS\r\n"); |
GaspardD | 14:d471faa7d1a2 | 519 | #endif |
GaspardD | 23:04d393220daa | 520 | |
GaspardD | 23:04d393220daa | 521 | differenceGD90 = distMurD90Moy - distMurG90Moy; |
GaspardD | 23:04d393220daa | 522 | differenceGD45 = distMurD45Moy - distMurG45Moy; |
GaspardD | 23:04d393220daa | 523 | |
GaspardD | 22:26fc6e6f7a55 | 524 | |
GaspardD | 21:de7a0a47f8a3 | 525 | //deriv correction |
GaspardD | 22:26fc6e6f7a55 | 526 | derive45 = differenceGD45 - prevDiffGD45; |
GaspardD | 22:26fc6e6f7a55 | 527 | derive90 = differenceGD90 - prevDiffGD90; |
GaspardD | 21:de7a0a47f8a3 | 528 | //integral correction |
GaspardD | 22:26fc6e6f7a55 | 529 | lastDifferences90[lastDifferenceIndex] = differenceGD90; |
GaspardD | 22:26fc6e6f7a55 | 530 | integralSum=0; |
GaspardD | 22:26fc6e6f7a55 | 531 | for(int f=0; f<NB_INTEGRAL_SAMPLES; f++) { |
GaspardD | 22:26fc6e6f7a55 | 532 | integralSum+=lastDifferences90[f]; |
GaspardD | 22:26fc6e6f7a55 | 533 | } |
GaspardD | 22:26fc6e6f7a55 | 534 | lastDifferenceIndex = (lastDifferenceIndex + 1)%NB_INTEGRAL_SAMPLES; |
GaspardD | 22:26fc6e6f7a55 | 535 | |
GaspardD | 21:de7a0a47f8a3 | 536 | //application des coefficients |
GaspardD | 21:de7a0a47f8a3 | 537 | pulseDirection_us = ( ((differenceGD90*2+differenceGD45*4)/8) / p_sectionCourante->coef_p) |
GaspardD | 23:04d393220daa | 538 | + ( derive45 / p_sectionCourante->coef_d) |
GaspardD | 21:de7a0a47f8a3 | 539 | + (integralSum / p_sectionCourante->coef_i) |
GaspardD | 22:26fc6e6f7a55 | 540 | + DIRECTION_PULSE_MIDDLE; |
GaspardD | 22:26fc6e6f7a55 | 541 | |
GaspardD | 21:de7a0a47f8a3 | 542 | //gestioon du dépassement |
GaspardD | 21:de7a0a47f8a3 | 543 | if(pulseDirection_us > DIRECTION_PULSE_MAX) { //POUR TOURNER A GAUCHE |
GaspardD | 19:771bf61be276 | 544 | #if DEBUG > 1 |
GaspardD | 21:de7a0a47f8a3 | 545 | pc.printf("!!! OVER PWM Direction pulse: %d\r\n",pulseDirection_us); |
GaspardD | 21:de7a0a47f8a3 | 546 | #endif |
GaspardD | 21:de7a0a47f8a3 | 547 | pulseDirection_us = DIRECTION_PULSE_MAX; |
GaspardD | 21:de7a0a47f8a3 | 548 | } else if(pulseDirection_us < DIRECTION_PULSE_MIN ) { //POUR TOURNER A DROITE |
GaspardD | 21:de7a0a47f8a3 | 549 | #if DEBUG > 1 |
GaspardD | 21:de7a0a47f8a3 | 550 | pc.printf("!!! UNDER PWM Direction pulse: %d\r\n",pulseDirection_us); |
GaspardD | 19:771bf61be276 | 551 | #endif |
GaspardD | 21:de7a0a47f8a3 | 552 | pulseDirection_us = DIRECTION_PULSE_MIN ; |
GaspardD | 21:de7a0a47f8a3 | 553 | } |
GaspardD | 21:de7a0a47f8a3 | 554 | #if DEBUG > 1 |
GaspardD | 21:de7a0a47f8a3 | 555 | pc.printf("PWM Direction pulse: %d\r\n",pulseDirection_us); |
GaspardD | 21:de7a0a47f8a3 | 556 | #endif |
GaspardD | 21:de7a0a47f8a3 | 557 | PwmDirection.pulsewidth_us(pulseDirection_us); |
GaspardD | 8:1d8c3ca5e508 | 558 | return; |
GaspardD | 8:1d8c3ca5e508 | 559 | } |
GaspardD | 8:1d8c3ca5e508 | 560 | |
GaspardD | 11:bc24b3ba51a9 | 561 | #ifdef DLVV |
GaspardD | 11:bc24b3ba51a9 | 562 | void obstacleOutput(void) |
GaspardD | 8:1d8c3ca5e508 | 563 | { |
GaspardD | 8:1d8c3ca5e508 | 564 | return; |
GaspardD | 8:1d8c3ca5e508 | 565 | } |
GaspardD | 11:bc24b3ba51a9 | 566 | #endif |
GaspardD | 11:bc24b3ba51a9 | 567 | void sectionOutput(void) |
GaspardD | 8:1d8c3ca5e508 | 568 | { |
GaspardD | 14:d471faa7d1a2 | 569 | #if (DEBUG > 3) |
GaspardD | 14:d471faa7d1a2 | 570 | pc.printf("\r\n Output Section\r\n"); |
GaspardD | 14:d471faa7d1a2 | 571 | #endif |
GaspardD | 11:bc24b3ba51a9 | 572 | switch (st_currentSection) { |
GaspardD | 19:771bf61be276 | 573 | case RUNNING_SECTION: |
GaspardD | 19:771bf61be276 | 574 | break; |
GaspardD | 19:771bf61be276 | 575 | case LOADING_SECTION: |
GaspardD | 19:771bf61be276 | 576 | p_sectionCourante=p_sectionCourante->nextSection; |
GaspardD | 19:771bf61be276 | 577 | tachySectionDist_cm = 0; |
GaspardD | 19:771bf61be276 | 578 | break; |
GaspardD | 19:771bf61be276 | 579 | case ARRET: |
GaspardD | 19:771bf61be276 | 580 | //on est à l'arret |
GaspardD | 19:771bf61be276 | 581 | break; |
GaspardD | 19:771bf61be276 | 582 | default: |
GaspardD | 19:771bf61be276 | 583 | break; |
GaspardD | 11:bc24b3ba51a9 | 584 | } |
GaspardD | 8:1d8c3ca5e508 | 585 | return; |
GaspardD | 8:1d8c3ca5e508 | 586 | } |
GaspardD | 8:1d8c3ca5e508 | 587 | |
GaspardD | 11:bc24b3ba51a9 | 588 | |
GaspardD | 11:bc24b3ba51a9 | 589 | void maxSpeedOutput(void) |
GaspardD | 8:1d8c3ca5e508 | 590 | { |
GaspardD | 14:d471faa7d1a2 | 591 | #if (DEBUG > 3) |
GaspardD | 14:d471faa7d1a2 | 592 | pc.printf("\r\n Output MAX SPEED\r\n"); |
GaspardD | 14:d471faa7d1a2 | 593 | #endif |
GaspardD | 19:771bf61be276 | 594 | switch(st_maxSpeed) { |
GaspardD | 21:de7a0a47f8a3 | 595 | case BLOCKED: |
GaspardD | 24:698fefbbee00 | 596 | maxSpeed_cmps = 0; |
GaspardD | 19:771bf61be276 | 597 | break; |
GaspardD | 21:de7a0a47f8a3 | 598 | case SPEED_WARNING: |
GaspardD | 24:698fefbbee00 | 599 | maxSpeed_cmps = p_sectionCourante->slowSpeed_cmps; |
GaspardD | 19:771bf61be276 | 600 | break; |
GaspardD | 19:771bf61be276 | 601 | default: |
GaspardD | 19:771bf61be276 | 602 | maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps; |
GaspardD | 19:771bf61be276 | 603 | break; |
GaspardD | 11:bc24b3ba51a9 | 604 | } |
GaspardD | 8:1d8c3ca5e508 | 605 | return; |
GaspardD | 8:1d8c3ca5e508 | 606 | } |
GaspardD | 11:bc24b3ba51a9 | 607 | |
GaspardD | 11:bc24b3ba51a9 | 608 | void throttleOutput(void) |
GaspardD | 11:bc24b3ba51a9 | 609 | { |
GaspardD | 14:d471faa7d1a2 | 610 | #if (DEBUG > 3) |
GaspardD | 14:d471faa7d1a2 | 611 | pc.printf("\r\n Output TROTTLE\r\n"); |
GaspardD | 14:d471faa7d1a2 | 612 | #endif |
GaspardD | 11:bc24b3ba51a9 | 613 | switch (st_thro) { |
GaspardD | 21:de7a0a47f8a3 | 614 | case REGULATION_SPEED: |
GaspardD | 22:26fc6e6f7a55 | 615 | pulseSpeed_us = maxSpeed_cmps * 279 / 2048 + 1558 ; |
GaspardD | 19:771bf61be276 | 616 | break; |
GaspardD | 19:771bf61be276 | 617 | case BRAKING: |
GaspardD | 16:63690703b5b6 | 618 | #if DEBUG > 2 |
GaspardD | 19:771bf61be276 | 619 | pc.printf("BRAKINGGGGGGGGGGGGGGGGGG !!! \r\n"); |
GaspardD | 19:771bf61be276 | 620 | #endif |
GaspardD | 19:771bf61be276 | 621 | pulseSpeed_us = BRAKING_PULSE_US; |
GaspardD | 19:771bf61be276 | 622 | break; |
GaspardD | 19:771bf61be276 | 623 | case STOPPED: |
GaspardD | 19:771bf61be276 | 624 | #if DEBUG > 2 |
GaspardD | 19:771bf61be276 | 625 | pc.printf("STOPPED\r\n"); |
GaspardD | 16:63690703b5b6 | 626 | #endif |
GaspardD | 19:771bf61be276 | 627 | pulseSpeed_us = ZERO_PULSE_SPEED_US; |
GaspardD | 19:771bf61be276 | 628 | break; |
GaspardD | 19:771bf61be276 | 629 | default: |
GaspardD | 19:771bf61be276 | 630 | break; |
GaspardD | 11:bc24b3ba51a9 | 631 | } |
GaspardD | 22:26fc6e6f7a55 | 632 | |
GaspardD | 19:771bf61be276 | 633 | PwmMotor.pulsewidth_us(pulseSpeed_us); |
GaspardD | 15:129f205ff030 | 634 | #if DEBUG > 1 |
GaspardD | 19:771bf61be276 | 635 | pc.printf("PWM Thro pulse: %d micros\r\n",pulseSpeed_us); |
GaspardD | 13:af9a59ccf60b | 636 | #endif |
GaspardD | 11:bc24b3ba51a9 | 637 | return; |
GaspardD | 11:bc24b3ba51a9 | 638 | } |