TRR 2018 / Mbed 2 deprecated biniou-TRR2019-DLVV

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MPU6050

sm_sections.cpp@15:0f89d1ff860d, 2019-10-05 (annotated)

Committer:
ohlimi2
Date:
Sat Oct 05 01:32:50 2019 +0000
Revision:
15:0f89d1ff860d
Parent:
14:914467165f34 
Pour le mordor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GaspardD 3:1b7eb426247e 1 #include "sm_sections.h"
GaspardD 8:f23601373e8b 2 #include "sm_mpu.h"
GaspardD 3:1b7eb426247e 3
GaspardD 3:1b7eb426247e 4
GaspardD 4:efa207509f63 5 s_Section s_section1;
GaspardD 4:efa207509f63 6 s_Section s_section2;
GaspardD 12:58ad06f9847d 7 s_Section s_section2_1;
GaspardD 11:2156cb77d0d6 8 s_Section s_section3;
GaspardD 12:58ad06f9847d 9 s_Section s_section3_1;
GaspardD 12:58ad06f9847d 10 s_Section s_section4;
GaspardD 12:58ad06f9847d 11 s_Section s_section5;
GaspardD 12:58ad06f9847d 12 s_Section s_section5_1;
GaspardD 12:58ad06f9847d 13 s_Section s_section6_0;
GaspardD 12:58ad06f9847d 14 s_Section s_section6;
GaspardD 13:3868cbdfa5ce 15 s_Section s_section6_1;
GaspardD 12:58ad06f9847d 16 s_Section s_section7;
GaspardD 12:58ad06f9847d 17 s_Section s_section8;
GaspardD 12:58ad06f9847d 18 s_Section s_section9;
GaspardD 12:58ad06f9847d 19 s_Section s_section9_1;
GaspardD 12:58ad06f9847d 20 s_Section s_section10;
GaspardD 12:58ad06f9847d 21 s_Section s_section11;
GaspardD 13:3868cbdfa5ce 22 s_Section s_section11_1;
ohlimi2 15:0f89d1ff860d 23 s_Section s_section11_2;
GaspardD 12:58ad06f9847d 24 s_Section s_section12;
GaspardD 3:1b7eb426247e 25
GaspardD 8:f23601373e8b 26 E_STATE_SECTIONS e_SECTIONS_state;
GaspardD 8:f23601373e8b 27
GaspardD 3:1b7eb426247e 28
GaspardD 8:f23601373e8b 29 void init_sm_sections()
GaspardD 8:f23601373e8b 30 {
GaspardD 3:1b7eb426247e 31
GaspardD 4:efa207509f63 32 d_ODOM_distFromSectionStart_m = 0.0;
GaspardD 8:f23601373e8b 33 e_SECTIONS_state = STOPPED;
GaspardD 8:f23601373e8b 34 s_section1.nextSection = NULL;
GaspardD 4:efa207509f63 35
GaspardD 4:efa207509f63 36
GaspardD 8:f23601373e8b 37 //section de départ
GaspardD 4:efa207509f63 38 s_section1.nextSection = &s_section2;
GaspardD 9:1b54bac6d9a7 39 s_section1.consigne_position = 0.5;
GaspardD 12:58ad06f9847d 40 s_section1.targetSpeed_mps = 1750;
GaspardD 12:58ad06f9847d 41 s_section1.lng_section_m = 0.8;
GaspardD 12:58ad06f9847d 42 s_section1.coef_pos = 40;
GaspardD 12:58ad06f9847d 43 s_section1.coef_bord = 0.6;
GaspardD 12:58ad06f9847d 44 s_section1.plagePulseFreinage = 20;
GaspardD 3:1b7eb426247e 45
GaspardD 12:58ad06f9847d 46 //ligne droite debut
GaspardD 12:58ad06f9847d 47 s_section2.nextSection = &s_section2_1;
GaspardD 9:1b54bac6d9a7 48 s_section2.consigne_position = 0.5;
ohlimi2 15:0f89d1ff860d 49 s_section2.targetSpeed_mps = 1790;
ohlimi2 15:0f89d1ff860d 50 s_section2.lng_section_m = 11.5;
GaspardD 12:58ad06f9847d 51 s_section2.coef_pos = 30;
GaspardD 12:58ad06f9847d 52 s_section2.coef_bord = 0.5;
GaspardD 12:58ad06f9847d 53 s_section2.plagePulseFreinage = 50;
GaspardD 12:58ad06f9847d 54
GaspardD 12:58ad06f9847d 55
GaspardD 12:58ad06f9847d 56 //ligne droite debut
GaspardD 12:58ad06f9847d 57 s_section2_1.nextSection = &s_section3;
ohlimi2 15:0f89d1ff860d 58 s_section2_1.consigne_position = 0.5;
ohlimi2 15:0f89d1ff860d 59 s_section2_1.targetSpeed_mps = 1670;
ohlimi2 15:0f89d1ff860d 60 s_section2_1.lng_section_m = 1.5;
GaspardD 12:58ad06f9847d 61 s_section2_1.coef_pos = 30;
ohlimi2 15:0f89d1ff860d 62 s_section2_1.coef_bord = 0.9;
GaspardD 12:58ad06f9847d 63 s_section2_1.plagePulseFreinage = 50;
GaspardD 12:58ad06f9847d 64
GaspardD 12:58ad06f9847d 65 //epingle 1 debut
GaspardD 12:58ad06f9847d 66 s_section3.nextSection = &s_section3_1;
GaspardD 12:58ad06f9847d 67 s_section3.consigne_position = 0.7;
ohlimi2 15:0f89d1ff860d 68 s_section3.targetSpeed_mps = 1660;
GaspardD 12:58ad06f9847d 69 s_section3.lng_section_m = 2.0;
GaspardD 12:58ad06f9847d 70 s_section3.coef_pos = 30;
GaspardD 12:58ad06f9847d 71 s_section3.coef_bord = 2.0;
ohlimi2 15:0f89d1ff860d 72 s_section3.plagePulseFreinage = 30;
GaspardD 12:58ad06f9847d 73
GaspardD 12:58ad06f9847d 74 //epingle 1 milieu
GaspardD 12:58ad06f9847d 75 s_section3_1.nextSection = &s_section4;
GaspardD 12:58ad06f9847d 76 s_section3_1.consigne_position = 0.7;
GaspardD 12:58ad06f9847d 77 s_section3_1.targetSpeed_mps = 1690;
GaspardD 12:58ad06f9847d 78 s_section3_1.lng_section_m = 4.0;
ohlimi2 15:0f89d1ff860d 79 s_section3_1.coef_pos = 40;
GaspardD 12:58ad06f9847d 80 s_section3_1.coef_bord = 2.0;
GaspardD 12:58ad06f9847d 81 s_section3_1.plagePulseFreinage = 20;
GaspardD 12:58ad06f9847d 82
GaspardD 12:58ad06f9847d 83 //post epingle 1
GaspardD 12:58ad06f9847d 84 s_section4.nextSection = &s_section5;
GaspardD 12:58ad06f9847d 85 s_section4.consigne_position = 0.8;
GaspardD 12:58ad06f9847d 86 s_section4.targetSpeed_mps = 1720;
GaspardD 12:58ad06f9847d 87 s_section4.lng_section_m = 5.0;
GaspardD 12:58ad06f9847d 88 s_section4.coef_pos = 30;
GaspardD 12:58ad06f9847d 89 s_section4.coef_bord = 1.3;
GaspardD 12:58ad06f9847d 90 s_section4.plagePulseFreinage = 30;
GaspardD 12:58ad06f9847d 91
GaspardD 12:58ad06f9847d 92 //droite epingle chicane
GaspardD 12:58ad06f9847d 93 s_section5.nextSection = &s_section5_1;
GaspardD 12:58ad06f9847d 94 s_section5.consigne_position = 0.6;
GaspardD 12:58ad06f9847d 95 s_section5.targetSpeed_mps = 1780;
ohlimi2 15:0f89d1ff860d 96 s_section5.lng_section_m = 8.0;
GaspardD 12:58ad06f9847d 97 s_section5.coef_pos = 30;
GaspardD 12:58ad06f9847d 98 s_section5.coef_bord = 0.7;
GaspardD 12:58ad06f9847d 99 s_section5.plagePulseFreinage = 40;
GaspardD 11:2156cb77d0d6 100
GaspardD 11:2156cb77d0d6 101
GaspardD 12:58ad06f9847d 102 //avant chicane
GaspardD 12:58ad06f9847d 103 s_section5_1.nextSection = &s_section6_0;
ohlimi2 15:0f89d1ff860d 104 s_section5_1.consigne_position = 0.7;
ohlimi2 15:0f89d1ff860d 105 s_section5_1.targetSpeed_mps = 1690;
GaspardD 12:58ad06f9847d 106 s_section5_1.lng_section_m = 4.0;
GaspardD 12:58ad06f9847d 107 s_section5_1.coef_pos = 30;
GaspardD 12:58ad06f9847d 108 s_section5_1.coef_bord = 2.0;
GaspardD 12:58ad06f9847d 109 s_section5_1.plagePulseFreinage = 40;
GaspardD 12:58ad06f9847d 110
GaspardD 12:58ad06f9847d 111 //pre chicane
GaspardD 12:58ad06f9847d 112 s_section6_0.nextSection = &s_section6;
GaspardD 12:58ad06f9847d 113 s_section6_0.consigne_position = 0.5;
ohlimi2 15:0f89d1ff860d 114 s_section6_0.targetSpeed_mps = 1660;
ohlimi2 15:0f89d1ff860d 115 s_section6_0.lng_section_m = 2.0;
ohlimi2 15:0f89d1ff860d 116 s_section6_0.coef_pos = 40;//50
ohlimi2 15:0f89d1ff860d 117 s_section6_0.coef_bord = 0.9;//2.0
GaspardD 12:58ad06f9847d 118 s_section6_0.plagePulseFreinage = 45;
GaspardD 12:58ad06f9847d 119
GaspardD 12:58ad06f9847d 120
GaspardD 12:58ad06f9847d 121 //chicane
GaspardD 12:58ad06f9847d 122 s_section6.nextSection = &s_section7;
ohlimi2 15:0f89d1ff860d 123 s_section6.consigne_position = 0.6;
GaspardD 12:58ad06f9847d 124 s_section6.targetSpeed_mps = 1675;
ohlimi2 15:0f89d1ff860d 125 s_section6.lng_section_m = 17.5;
ohlimi2 15:0f89d1ff860d 126 s_section6.coef_pos = 40;//50
GaspardD 12:58ad06f9847d 127 s_section6.coef_bord = 2.0;//2.0
GaspardD 12:58ad06f9847d 128 s_section6.plagePulseFreinage = 50;
GaspardD 12:58ad06f9847d 129
GaspardD 13:3868cbdfa5ce 130
GaspardD 12:58ad06f9847d 131 //ligne droite postchicane
GaspardD 12:58ad06f9847d 132 s_section7.nextSection = &s_section8;
GaspardD 13:3868cbdfa5ce 133 s_section7.consigne_position = 0.4;
ohlimi2 15:0f89d1ff860d 134 s_section7.targetSpeed_mps = 1730;
ohlimi2 14:914467165f34 135 s_section7.lng_section_m = 8.0;
GaspardD 12:58ad06f9847d 136 s_section7.coef_pos = 30;
GaspardD 12:58ad06f9847d 137 s_section7.coef_bord = 0.5;
GaspardD 12:58ad06f9847d 138 s_section7.plagePulseFreinage = 50;
GaspardD 12:58ad06f9847d 139
GaspardD 12:58ad06f9847d 140
GaspardD 13:3868cbdfa5ce 141 //freinage avant epingle
GaspardD 12:58ad06f9847d 142 s_section8.nextSection = &s_section9;
ohlimi2 15:0f89d1ff860d 143 s_section8.consigne_position = 0.5;
ohlimi2 15:0f89d1ff860d 144 s_section8.targetSpeed_mps = 1660;
ohlimi2 15:0f89d1ff860d 145 s_section8.lng_section_m = 3.5;
GaspardD 12:58ad06f9847d 146 s_section8.coef_pos = 30;
ohlimi2 15:0f89d1ff860d 147 s_section8.coef_bord = 0.5;
GaspardD 12:58ad06f9847d 148 s_section8.plagePulseFreinage = 50;
GaspardD 12:58ad06f9847d 149
GaspardD 12:58ad06f9847d 150 //epingle 2 debut
GaspardD 12:58ad06f9847d 151 s_section9.nextSection = &s_section9_1;
ohlimi2 15:0f89d1ff860d 152 s_section9.consigne_position = 0.6;
ohlimi2 15:0f89d1ff860d 153 s_section9.targetSpeed_mps = 1660;
GaspardD 12:58ad06f9847d 154 s_section9.lng_section_m = 2.0;
ohlimi2 15:0f89d1ff860d 155 s_section9.coef_pos = 40;
GaspardD 12:58ad06f9847d 156 s_section9.coef_bord = 2.0;
GaspardD 12:58ad06f9847d 157 s_section9.plagePulseFreinage = 20;
GaspardD 12:58ad06f9847d 158
GaspardD 12:58ad06f9847d 159 //epingle 2 milieu
GaspardD 12:58ad06f9847d 160 s_section9_1.nextSection = &s_section10;
GaspardD 12:58ad06f9847d 161 s_section9_1.consigne_position = 0.7;
GaspardD 13:3868cbdfa5ce 162 s_section9_1.targetSpeed_mps = 1670;
ohlimi2 15:0f89d1ff860d 163 s_section9_1.lng_section_m = 5.0;
ohlimi2 15:0f89d1ff860d 164 s_section9_1.coef_pos = 40;
GaspardD 12:58ad06f9847d 165 s_section9_1.coef_bord = 2.0;
GaspardD 12:58ad06f9847d 166 s_section9_1.plagePulseFreinage = 20;
GaspardD 12:58ad06f9847d 167
GaspardD 12:58ad06f9847d 168 //post epingle 2
GaspardD 12:58ad06f9847d 169 s_section10.nextSection = &s_section11;
ohlimi2 15:0f89d1ff860d 170 s_section10.consigne_position = 0.5;
ohlimi2 15:0f89d1ff860d 171 s_section10.targetSpeed_mps = 1700;
GaspardD 12:58ad06f9847d 172 s_section10.lng_section_m = 5.0;
GaspardD 12:58ad06f9847d 173 s_section10.coef_pos = 30;
ohlimi2 15:0f89d1ff860d 174 s_section10.coef_bord = 1.5;
GaspardD 12:58ad06f9847d 175 s_section10.plagePulseFreinage = 30;
GaspardD 12:58ad06f9847d 176
GaspardD 12:58ad06f9847d 177 //dernière ligne droite
GaspardD 13:3868cbdfa5ce 178 s_section11.nextSection = &s_section11_1;
ohlimi2 15:0f89d1ff860d 179 s_section11.consigne_position = 0.5;
ohlimi2 15:0f89d1ff860d 180 s_section11.targetSpeed_mps = 1800;
ohlimi2 15:0f89d1ff860d 181 s_section11.lng_section_m = 5.0;
ohlimi2 15:0f89d1ff860d 182 s_section11.coef_pos = 30;
ohlimi2 15:0f89d1ff860d 183 s_section11.coef_bord = 0.5;
ohlimi2 14:914467165f34 184 s_section11.plagePulseFreinage = 40;
GaspardD 13:3868cbdfa5ce 185
GaspardD 13:3868cbdfa5ce 186 //dernière ligne droite
ohlimi2 15:0f89d1ff860d 187 s_section11_1.nextSection = &s_section11_2;
GaspardD 13:3868cbdfa5ce 188 s_section11_1.consigne_position = 0.5;
ohlimi2 15:0f89d1ff860d 189 s_section11_1.targetSpeed_mps = 1870;
ohlimi2 15:0f89d1ff860d 190 s_section11_1.lng_section_m = 5.0;
ohlimi2 14:914467165f34 191 s_section11_1.coef_pos = 20;
ohlimi2 15:0f89d1ff860d 192 s_section11_1.coef_bord = 0.5;
GaspardD 13:3868cbdfa5ce 193 s_section11_1.plagePulseFreinage = 100;
GaspardD 12:58ad06f9847d 194
ohlimi2 15:0f89d1ff860d 195 //der des dernière ligne droite
ohlimi2 15:0f89d1ff860d 196 s_section11_2.nextSection = &s_section12;
ohlimi2 15:0f89d1ff860d 197 s_section11_2.consigne_position = 0.5;
ohlimi2 15:0f89d1ff860d 198 s_section11_2.targetSpeed_mps = 1900;
ohlimi2 15:0f89d1ff860d 199 s_section11_2.lng_section_m = 25.0;
ohlimi2 15:0f89d1ff860d 200 s_section11_2.coef_pos = 20;
ohlimi2 15:0f89d1ff860d 201 s_section11_2.coef_bord = 0.5;
ohlimi2 15:0f89d1ff860d 202 s_section11_2.plagePulseFreinage = 100;
GaspardD 12:58ad06f9847d 203
GaspardD 12:58ad06f9847d 204 //frein
GaspardD 12:58ad06f9847d 205 s_section12.nextSection = NULL;
GaspardD 12:58ad06f9847d 206 s_section12.consigne_position = 0.5;
ohlimi2 15:0f89d1ff860d 207 s_section12.targetSpeed_mps = 1500;
ohlimi2 15:0f89d1ff860d 208 s_section12.lng_section_m = 7.0;
ohlimi2 15:0f89d1ff860d 209 s_section12.coef_pos = 40;
GaspardD 12:58ad06f9847d 210 s_section12.coef_bord = 1.0;
GaspardD 12:58ad06f9847d 211 s_section12.plagePulseFreinage = 20;
GaspardD 3:1b7eb426247e 212
GaspardD 3:1b7eb426247e 213 return;
GaspardD 3:1b7eb426247e 214 }
GaspardD 8:f23601373e8b 215 void update_sm_sections()
GaspardD 8:f23601373e8b 216 {
GaspardD 8:f23601373e8b 217
GaspardD 8:f23601373e8b 218 E_STATE_SECTIONS e_next_state = e_SECTIONS_state;
GaspardD 4:efa207509f63 219
GaspardD 9:1b54bac6d9a7 220 //rs_LOG_pc.printf("\r\nUpdate Section\r\n");
GaspardD 8:f23601373e8b 221
GaspardD 8:f23601373e8b 222 if(b_UTILS_flag_emergency_stop) {
GaspardD 12:58ad06f9847d 223 rs_LOG.printf("emergency stop -> STOPPED\r\n");
GaspardD 4:efa207509f63 224 e_next_state = STOPPED;
GaspardD 4:efa207509f63 225 } else {
GaspardD 8:f23601373e8b 226 switch (e_SECTIONS_state) {
GaspardD 4:efa207509f63 227 case INIT_SECTION:
GaspardD 8:f23601373e8b 228 e_next_state = RUNNING_SECTION;
GaspardD 9:1b54bac6d9a7 229 //rs_LOG_pc.printf("RUNNING SECTION\r\n");
GaspardD 8:f23601373e8b 230 break;
GaspardD 4:efa207509f63 231 case RUNNING_SECTION:
GaspardD 6:ab9f3695633f 232 if(d_ODOM_distFromSectionStart_m > s_UTILS_currentSection->lng_section_m) {
GaspardD 4:efa207509f63 233 e_next_state = LOADING_SECTION;
GaspardD 9:1b54bac6d9a7 234 //rs_LOG_pc.printf("LOADING SECTION !\r\n");
GaspardD 4:efa207509f63 235 }
GaspardD 4:efa207509f63 236 break;
GaspardD 4:efa207509f63 237 case LOADING_SECTION:
GaspardD 8:f23601373e8b 238 if(s_UTILS_currentSection != NULL) {
GaspardD 4:efa207509f63 239 e_next_state = RUNNING_SECTION;
GaspardD 9:1b54bac6d9a7 240 //rs_LOG_pc.printf("RUNNING SECTION !\r\n");
GaspardD 4:efa207509f63 241 } else {
GaspardD 8:f23601373e8b 242 b_UTILS_flag_emergency_stop = true;
GaspardD 11:2156cb77d0d6 243 log_stop();
GaspardD 8:f23601373e8b 244 e_next_state = STOPPED;
GaspardD 4:efa207509f63 245 }
GaspardD 4:efa207509f63 246 break;
GaspardD 4:efa207509f63 247 case STOPPED:
GaspardD 8:f23601373e8b 248 if(b_UTILS_flag_button_SECTIONS) {
GaspardD 8:f23601373e8b 249 e_next_state = INIT_SECTION;
GaspardD 12:58ad06f9847d 250 rs_LOG.printf("INIT SECTION !\r\n");
GaspardD 4:efa207509f63 251 }
GaspardD 4:efa207509f63 252 break;
GaspardD 4:efa207509f63 253 default:
GaspardD 4:efa207509f63 254 break;
GaspardD 4:efa207509f63 255 }
GaspardD 4:efa207509f63 256 }
GaspardD 8:f23601373e8b 257 e_SECTIONS_state = e_next_state;
GaspardD 3:1b7eb426247e 258 return;
GaspardD 3:1b7eb426247e 259 }
GaspardD 8:f23601373e8b 260 void output_sm_sections()
GaspardD 8:f23601373e8b 261 {
GaspardD 8:f23601373e8b 262 switch (e_SECTIONS_state) {
GaspardD 8:f23601373e8b 263 case INIT_SECTION:
GaspardD 8:f23601373e8b 264 b_UTILS_flag_button_SECTIONS = false;
GaspardD 6:ab9f3695633f 265 s_UTILS_currentSection = &s_section1;
GaspardD 12:58ad06f9847d 266 rs_LOG.printf("sect 1 INITIALIZED\r\n");
GaspardD 8:f23601373e8b 267 case RUNNING_SECTION:
GaspardD 8:f23601373e8b 268 break;
GaspardD 8:f23601373e8b 269 case LOADING_SECTION:
GaspardD 9:1b54bac6d9a7 270 //rs_LOG_pc.printf("next section loading\r\n");
GaspardD 8:f23601373e8b 271 s_UTILS_currentSection=s_UTILS_currentSection->nextSection;
GaspardD 8:f23601373e8b 272 d_ODOM_distFromSectionStart_m = 0.0;
GaspardD 8:f23601373e8b 273 break;
GaspardD 8:f23601373e8b 274 case STOPPED:
GaspardD 8:f23601373e8b 275 //on est à l'arret
GaspardD 8:f23601373e8b 276 break;
GaspardD 8:f23601373e8b 277 default:
GaspardD 8:f23601373e8b 278 break;
GaspardD 4:efa207509f63 279 }
GaspardD 3:1b7eb426247e 280 return;
GaspardD 3:1b7eb426247e 281 }