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@4:efa207509f63, 2019-09-29 (annotated)

Committer:
GaspardD
Date:
Sun Sep 29 14:58:03 2019 +0000
Revision:
4:efa207509f63
Parent:
3:1b7eb426247e
Child:
6:ab9f3695633f
algo fonctionnel direction + esc rotation a droite 10 sec a vitesse minimale

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GaspardD 3:1b7eb426247e 1 #include "sm_sections.h"
GaspardD 3:1b7eb426247e 2
GaspardD 4:efa207509f63 3 s_Section* s_currentSection;
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 4:efa207509f63 8 E_STATE_SECTIONS e_stateSections;
GaspardD 3:1b7eb426247e 9
GaspardD 3:1b7eb426247e 10 void init_sm_sections(){
GaspardD 3:1b7eb426247e 11
GaspardD 4:efa207509f63 12 d_ODOM_distFromSectionStart_m = 0.0;
GaspardD 4:efa207509f63 13 e_stateSections = STOPPED;
GaspardD 4:efa207509f63 14 s_currentSection = NULL;
GaspardD 4:efa207509f63 15
GaspardD 4:efa207509f63 16
GaspardD 3:1b7eb426247e 17 //section de départ
GaspardD 4:efa207509f63 18 s_section1.nextSection = &s_section2;
GaspardD 4:efa207509f63 19 s_section1.consigne_position = 0.75;
GaspardD 4:efa207509f63 20 s_section1.targetSpeed_mps = 1.0;
GaspardD 4:efa207509f63 21 s_section1.lng_section_m = 1.0;
GaspardD 4:efa207509f63 22 s_section1.coef_p = 1.0;
GaspardD 4:efa207509f63 23 s_section1.coef_d = 0.0;
GaspardD 4:efa207509f63 24 s_section1.coef_i = 0.000;
GaspardD 3:1b7eb426247e 25
GaspardD 3:1b7eb426247e 26 //epingle 1
GaspardD 4:efa207509f63 27 s_section2.nextSection = NULL;
GaspardD 4:efa207509f63 28 s_section2.consigne_position = 0.75;
GaspardD 4:efa207509f63 29 s_section2.targetSpeed_mps = 1.0;
GaspardD 4:efa207509f63 30 s_section2.lng_section_m = 1.0;
GaspardD 4:efa207509f63 31 s_section2.coef_p = 1.0;
GaspardD 4:efa207509f63 32 s_section2.coef_d = 0.0;
GaspardD 4:efa207509f63 33 s_section2.coef_i = 0.000;
GaspardD 3:1b7eb426247e 34
GaspardD 3:1b7eb426247e 35 return;
GaspardD 3:1b7eb426247e 36 }
GaspardD 3:1b7eb426247e 37 void update_sm_sections(){
GaspardD 4:efa207509f63 38
GaspardD 4:efa207509f63 39 E_STATE_SECTIONS e_next_state = e_stateSections;
GaspardD 4:efa207509f63 40
GaspardD 4:efa207509f63 41 pc.printf("\r\nUpdate Section\r\n");
GaspardD 4:efa207509f63 42
GaspardD 4:efa207509f63 43 if(b_UTILS_flag_button){
GaspardD 4:efa207509f63 44 e_next_state = INIT_SECTION;
GaspardD 4:efa207509f63 45 }
GaspardD 4:efa207509f63 46 else if(s_currentSection == NULL && e_stateSections != INIT_SECTION) {
GaspardD 4:efa207509f63 47 e_next_state = STOPPED;
GaspardD 4:efa207509f63 48 } else {
GaspardD 4:efa207509f63 49
GaspardD 4:efa207509f63 50 switch (e_stateSections) {
GaspardD 4:efa207509f63 51 case INIT_SECTION:
GaspardD 4:efa207509f63 52 e_next_state = LOADING_SECTION;
GaspardD 4:efa207509f63 53 break;
GaspardD 4:efa207509f63 54 case RUNNING_SECTION:
GaspardD 4:efa207509f63 55 if(d_ODOM_distFromSectionStart_m > s_currentSection->lng_section_m) {
GaspardD 4:efa207509f63 56 e_next_state = LOADING_SECTION;
GaspardD 4:efa207509f63 57 } else if (b_UTILS_flag_emergency_stop){
GaspardD 4:efa207509f63 58 e_next_state = STOPPED;
GaspardD 4:efa207509f63 59 }
GaspardD 4:efa207509f63 60 else{
GaspardD 4:efa207509f63 61 return;
GaspardD 4:efa207509f63 62 }
GaspardD 4:efa207509f63 63 break;
GaspardD 4:efa207509f63 64 case LOADING_SECTION:
GaspardD 4:efa207509f63 65 if(s_currentSection != NULL && !b_UTILS_flag_emergency_stop) { //la section a ete chargee dans sectionOutput
GaspardD 4:efa207509f63 66 e_next_state = RUNNING_SECTION;
GaspardD 4:efa207509f63 67 } else {
GaspardD 4:efa207509f63 68 e_next_state=STOPPED;
GaspardD 4:efa207509f63 69 }
GaspardD 4:efa207509f63 70 break;
GaspardD 4:efa207509f63 71 case STOPPED:
GaspardD 4:efa207509f63 72 if(s_currentSection != NULL) {
GaspardD 4:efa207509f63 73 e_next_state = RUNNING_SECTION;
GaspardD 4:efa207509f63 74 } else {
GaspardD 4:efa207509f63 75 return;
GaspardD 4:efa207509f63 76 }
GaspardD 4:efa207509f63 77 break;
GaspardD 4:efa207509f63 78 default:
GaspardD 4:efa207509f63 79 break;
GaspardD 4:efa207509f63 80 }
GaspardD 4:efa207509f63 81 }
GaspardD 4:efa207509f63 82 e_stateSections = e_next_state;
GaspardD 3:1b7eb426247e 83 return;
GaspardD 3:1b7eb426247e 84 }
GaspardD 3:1b7eb426247e 85 void output_sm_sections(){
GaspardD 4:efa207509f63 86
GaspardD 4:efa207509f63 87 pc.printf("\r\n Output Section\r\n");
GaspardD 4:efa207509f63 88 if(s_currentSection !=NULL) {
GaspardD 4:efa207509f63 89 switch (e_stateSections) {
GaspardD 4:efa207509f63 90 case INIT_SECTION:
GaspardD 4:efa207509f63 91 b_UTILS_flag_button = false;
GaspardD 4:efa207509f63 92 s_currentSection = &s_section1;
GaspardD 4:efa207509f63 93 case RUNNING_SECTION:
GaspardD 4:efa207509f63 94 break;
GaspardD 4:efa207509f63 95 case LOADING_SECTION:
GaspardD 4:efa207509f63 96 s_currentSection=s_currentSection->nextSection;
GaspardD 4:efa207509f63 97 d_ODOM_distFromSectionStart_m = 0.0;
GaspardD 4:efa207509f63 98 break;
GaspardD 4:efa207509f63 99 case STOPPED:
GaspardD 4:efa207509f63 100 //on est à l'arret
GaspardD 4:efa207509f63 101 break;
GaspardD 4:efa207509f63 102 default:
GaspardD 4:efa207509f63 103 break;
GaspardD 4:efa207509f63 104 }
GaspardD 4:efa207509f63 105 }
GaspardD 3:1b7eb426247e 106 return;
GaspardD 3:1b7eb426247e 107 }