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@9:1b54bac6d9a7, 2019-10-03 (annotated)

Committer:
GaspardD
Date:
Thu Oct 03 23:28:56 2019 +0000
Revision:
9:1b54bac6d9a7
Parent:
8:f23601373e8b
Child:
10:e63fe4080760
a tester ;

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 3:1b7eb426247e 7
GaspardD 8:f23601373e8b 8 E_STATE_SECTIONS e_SECTIONS_state;
GaspardD 8:f23601373e8b 9
GaspardD 3:1b7eb426247e 10
GaspardD 8:f23601373e8b 11 void init_sm_sections()
GaspardD 8:f23601373e8b 12 {
GaspardD 3:1b7eb426247e 13
GaspardD 4:efa207509f63 14 d_ODOM_distFromSectionStart_m = 0.0;
GaspardD 8:f23601373e8b 15 e_SECTIONS_state = STOPPED;
GaspardD 8:f23601373e8b 16 s_section1.nextSection = NULL;
GaspardD 4:efa207509f63 17
GaspardD 4:efa207509f63 18
GaspardD 8:f23601373e8b 19 //section de départ
GaspardD 4:efa207509f63 20 s_section1.nextSection = &s_section2;
GaspardD 9:1b54bac6d9a7 21 s_section1.consigne_position = 0.5;
GaspardD 8:f23601373e8b 22 s_section1.targetSpeed_mps = 1640;
GaspardD 9:1b54bac6d9a7 23 s_section1.lng_section_m = 2.0;
GaspardD 4:efa207509f63 24 s_section1.coef_p = 1.0;
GaspardD 4:efa207509f63 25 s_section1.coef_d = 0.0;
GaspardD 4:efa207509f63 26 s_section1.coef_i = 0.000;
GaspardD 3:1b7eb426247e 27
GaspardD 3:1b7eb426247e 28 //epingle 1
GaspardD 4:efa207509f63 29 s_section2.nextSection = NULL;
GaspardD 9:1b54bac6d9a7 30 s_section2.consigne_position = 0.5;
GaspardD 8:f23601373e8b 31 s_section2.targetSpeed_mps = 1640;
GaspardD 9:1b54bac6d9a7 32 s_section2.lng_section_m = 2.0;
GaspardD 4:efa207509f63 33 s_section2.coef_p = 1.0;
GaspardD 4:efa207509f63 34 s_section2.coef_d = 0.0;
GaspardD 4:efa207509f63 35 s_section2.coef_i = 0.000;
GaspardD 3:1b7eb426247e 36
GaspardD 3:1b7eb426247e 37 return;
GaspardD 3:1b7eb426247e 38 }
GaspardD 8:f23601373e8b 39 void update_sm_sections()
GaspardD 8:f23601373e8b 40 {
GaspardD 8:f23601373e8b 41
GaspardD 8:f23601373e8b 42 E_STATE_SECTIONS e_next_state = e_SECTIONS_state;
GaspardD 4:efa207509f63 43
GaspardD 9:1b54bac6d9a7 44 //rs_LOG_pc.printf("\r\nUpdate Section\r\n");
GaspardD 8:f23601373e8b 45
GaspardD 8:f23601373e8b 46 if(b_UTILS_flag_emergency_stop) {
GaspardD 9:1b54bac6d9a7 47 rs_LOG_pc.printf("emergency stop -> STOPPED\r\n");
GaspardD 4:efa207509f63 48 e_next_state = STOPPED;
GaspardD 4:efa207509f63 49 } else {
GaspardD 8:f23601373e8b 50 switch (e_SECTIONS_state) {
GaspardD 4:efa207509f63 51 case INIT_SECTION:
GaspardD 8:f23601373e8b 52 e_next_state = RUNNING_SECTION;
GaspardD 9:1b54bac6d9a7 53 //rs_LOG_pc.printf("RUNNING SECTION\r\n");
GaspardD 8:f23601373e8b 54 break;
GaspardD 4:efa207509f63 55 case RUNNING_SECTION:
GaspardD 6:ab9f3695633f 56 if(d_ODOM_distFromSectionStart_m > s_UTILS_currentSection->lng_section_m) {
GaspardD 4:efa207509f63 57 e_next_state = LOADING_SECTION;
GaspardD 9:1b54bac6d9a7 58 //rs_LOG_pc.printf("LOADING SECTION !\r\n");
GaspardD 4:efa207509f63 59 }
GaspardD 4:efa207509f63 60 break;
GaspardD 4:efa207509f63 61 case LOADING_SECTION:
GaspardD 8:f23601373e8b 62 if(s_UTILS_currentSection != NULL) {
GaspardD 4:efa207509f63 63 e_next_state = RUNNING_SECTION;
GaspardD 9:1b54bac6d9a7 64 //rs_LOG_pc.printf("RUNNING SECTION !\r\n");
GaspardD 4:efa207509f63 65 } else {
GaspardD 8:f23601373e8b 66 b_UTILS_flag_emergency_stop = true;
GaspardD 8:f23601373e8b 67 e_next_state = STOPPED;
GaspardD 4:efa207509f63 68 }
GaspardD 4:efa207509f63 69 break;
GaspardD 4:efa207509f63 70 case STOPPED:
GaspardD 8:f23601373e8b 71 if(b_UTILS_flag_button_SECTIONS) {
GaspardD 8:f23601373e8b 72 e_next_state = INIT_SECTION;
GaspardD 9:1b54bac6d9a7 73 rs_LOG_pc.printf("INIT SECTION !\r\n");
GaspardD 4:efa207509f63 74 }
GaspardD 4:efa207509f63 75 break;
GaspardD 4:efa207509f63 76 default:
GaspardD 4:efa207509f63 77 break;
GaspardD 4:efa207509f63 78 }
GaspardD 4:efa207509f63 79 }
GaspardD 8:f23601373e8b 80 e_SECTIONS_state = e_next_state;
GaspardD 3:1b7eb426247e 81 return;
GaspardD 3:1b7eb426247e 82 }
GaspardD 8:f23601373e8b 83 void output_sm_sections()
GaspardD 8:f23601373e8b 84 {
GaspardD 8:f23601373e8b 85 switch (e_SECTIONS_state) {
GaspardD 8:f23601373e8b 86 case INIT_SECTION:
GaspardD 8:f23601373e8b 87 b_UTILS_flag_button_SECTIONS = false;
GaspardD 6:ab9f3695633f 88 s_UTILS_currentSection = &s_section1;
GaspardD 9:1b54bac6d9a7 89 rs_LOG_pc.printf("sect 1 INITIALIZED\r\n");
GaspardD 8:f23601373e8b 90 case RUNNING_SECTION:
GaspardD 8:f23601373e8b 91 break;
GaspardD 8:f23601373e8b 92 case LOADING_SECTION:
GaspardD 9:1b54bac6d9a7 93 //rs_LOG_pc.printf("next section loading\r\n");
GaspardD 8:f23601373e8b 94 s_UTILS_currentSection=s_UTILS_currentSection->nextSection;
GaspardD 8:f23601373e8b 95 d_ODOM_distFromSectionStart_m = 0.0;
GaspardD 8:f23601373e8b 96 break;
GaspardD 8:f23601373e8b 97 case STOPPED:
GaspardD 8:f23601373e8b 98 //on est à l'arret
GaspardD 8:f23601373e8b 99 break;
GaspardD 8:f23601373e8b 100 default:
GaspardD 8:f23601373e8b 101 break;
GaspardD 4:efa207509f63 102 }
GaspardD 3:1b7eb426247e 103 return;
GaspardD 3:1b7eb426247e 104 }