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@11:2156cb77d0d6, 2019-10-04 (annotated)

Committer:
GaspardD
Date:
Fri Oct 04 09:58:51 2019 +0000
Revision:
11:2156cb77d0d6
Parent:
10:e63fe4080760
Child:
12:58ad06f9847d
version avec log et tout

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