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_esc.cpp@12:58ad06f9847d, 2019-10-04 (annotated)

Committer:
GaspardD
Date:
Fri Oct 04 22:20:23 2019 +0000
Revision:
12:58ad06f9847d
Parent:
9:1b54bac6d9a7 
dernier code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GaspardD 1:8faddee0e52f 1 #include "sm_esc.h"
GaspardD 6:ab9f3695633f 2 #include "sm_sections.h"
GaspardD 12:58ad06f9847d 3 #include "sm_servo.h"
GaspardD 1:8faddee0e52f 4
GaspardD 1:8faddee0e52f 5 PwmOut pwm_ESC(PD_12);
GaspardD 1:8faddee0e52f 6 DigitalOut relay_ESC(PG_0);
GaspardD 8:f23601373e8b 7 E_STATE_ESC e_ESC_state;
GaspardD 1:8faddee0e52f 8
GaspardD 1:8faddee0e52f 9 void init_sm_esc()
GaspardD 1:8faddee0e52f 10 {
GaspardD 8:f23601373e8b 11 e_ESC_state = ESC_DISABLED;
GaspardD 1:8faddee0e52f 12 return;
GaspardD 1:8faddee0e52f 13 }
GaspardD 1:8faddee0e52f 14
GaspardD 1:8faddee0e52f 15 void update_sm_esc()
GaspardD 1:8faddee0e52f 16 {
GaspardD 8:f23601373e8b 17 E_STATE_ESC next_state = e_ESC_state;
GaspardD 1:8faddee0e52f 18
GaspardD 8:f23601373e8b 19 switch(e_ESC_state) {
GaspardD 1:8faddee0e52f 20 case ESC_DISABLED:
GaspardD 8:f23601373e8b 21 if(b_UTILS_flag_button_ESC && !b_UTILS_flag_emergency_stop) {
GaspardD 1:8faddee0e52f 22 next_state = ESC_INIT;
GaspardD 1:8faddee0e52f 23 }
GaspardD 1:8faddee0e52f 24 break;
GaspardD 1:8faddee0e52f 25 case ESC_INIT:
GaspardD 1:8faddee0e52f 26 next_state = ESC_COMMAND;
GaspardD 1:8faddee0e52f 27 break;
GaspardD 1:8faddee0e52f 28 case ESC_COMMAND:
GaspardD 5:8bbe640528bc 29 if(b_UTILS_flag_emergency_stop) {
GaspardD 5:8bbe640528bc 30 next_state = ESC_DISABLED;
GaspardD 5:8bbe640528bc 31 }
GaspardD 1:8faddee0e52f 32 break;
GaspardD 1:8faddee0e52f 33 default:
GaspardD 5:8bbe640528bc 34 break;
GaspardD 1:8faddee0e52f 35 }
GaspardD 1:8faddee0e52f 36
GaspardD 8:f23601373e8b 37 e_ESC_state = next_state;
GaspardD 1:8faddee0e52f 38 return;
GaspardD 1:8faddee0e52f 39 }
GaspardD 1:8faddee0e52f 40
GaspardD 1:8faddee0e52f 41 void output_sm_esc()
GaspardD 1:8faddee0e52f 42 {
GaspardD 8:f23601373e8b 43 switch(e_ESC_state) {
GaspardD 1:8faddee0e52f 44 case ESC_DISABLED:
GaspardD 5:8bbe640528bc 45 if(b_UTILS_flag_emergency_stop) {
GaspardD 12:58ad06f9847d 46 rs_LOG.printf("EMERGENCY STOP\r\n");
GaspardD 4:efa207509f63 47 b_UTILS_flag_emergency_stop = false;
GaspardD 8:f23601373e8b 48 if(b_CHASSIS_with_pushbutton) {
GaspardD 5:8bbe640528bc 49 relay_ESC = 1;
GaspardD 5:8bbe640528bc 50 wait(0.5);
GaspardD 5:8bbe640528bc 51 }
GaspardD 3:1b7eb426247e 52 relay_ESC = 0;
GaspardD 3:1b7eb426247e 53 }
GaspardD 1:8faddee0e52f 54 break;
GaspardD 5:8bbe640528bc 55
GaspardD 1:8faddee0e52f 56 case ESC_INIT:
GaspardD 12:58ad06f9847d 57 rs_LOG.printf("Init esc\r\n");
GaspardD 8:f23601373e8b 58 b_UTILS_flag_button_ESC = false;
GaspardD 3:1b7eb426247e 59 pwm_ESC.period_ms(ESC_PERIOD_DURATION_MS); //20 ms is default
GaspardD 3:1b7eb426247e 60 pwm_ESC.pulsewidth_us(0);
GaspardD 5:8bbe640528bc 61
GaspardD 1:8faddee0e52f 62 relay_ESC = 1;
GaspardD 8:f23601373e8b 63 if(b_CHASSIS_with_pushbutton) {
GaspardD 6:ab9f3695633f 64 wait(0.5);
GaspardD 6:ab9f3695633f 65 relay_ESC = 0;
GaspardD 5:8bbe640528bc 66 }
GaspardD 3:1b7eb426247e 67 wait(0.1);
GaspardD 5:8bbe640528bc 68
GaspardD 3:1b7eb426247e 69 pwm_ESC.pulsewidth_us(ESC_PULSE_MIN_US);
GaspardD 3:1b7eb426247e 70 wait(3);
GaspardD 3:1b7eb426247e 71 pwm_ESC.pulsewidth_us(ESC_PULSE_MAX_US);
GaspardD 3:1b7eb426247e 72 wait(1);
GaspardD 3:1b7eb426247e 73 pwm_ESC.pulsewidth_us(ESC_PULSE_IDLE_US);
GaspardD 1:8faddee0e52f 74 wait(1);
GaspardD 12:58ad06f9847d 75 log_start(120000);
GaspardD 1:8faddee0e52f 76 break;
GaspardD 5:8bbe640528bc 77
GaspardD 1:8faddee0e52f 78 case ESC_COMMAND:
GaspardD 8:f23601373e8b 79 switch(e_SECTIONS_state) {
GaspardD 6:ab9f3695633f 80 case RUNNING_SECTION:
GaspardD 12:58ad06f9847d 81 pwm_ESC.pulsewidth_us(s_UTILS_currentSection->targetSpeed_mps - (d_SERVO_coefFreinage*s_UTILS_currentSection->plagePulseFreinage));
GaspardD 6:ab9f3695633f 82 break;
GaspardD 6:ab9f3695633f 83 case STOPPED:
GaspardD 6:ab9f3695633f 84 break;
GaspardD 6:ab9f3695633f 85 default:
GaspardD 6:ab9f3695633f 86 break;
GaspardD 8:f23601373e8b 87 }
GaspardD 1:8faddee0e52f 88 break;
GaspardD 1:8faddee0e52f 89 default:
GaspardD 5:8bbe640528bc 90 break;
GaspardD 1:8faddee0e52f 91 }
GaspardD 5:8bbe640528bc 92
GaspardD 1:8faddee0e52f 93 return;
GaspardD 1:8faddee0e52f 94 }