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