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@8:f23601373e8b, 2019-10-02 (annotated)

Committer:
GaspardD
Date:
Wed Oct 02 22:25:12 2019 +0000
Revision:
8:f23601373e8b
Parent:
7:23bb20892c8c
Child:
9:1b54bac6d9a7
angle correction working

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 1:8faddee0e52f 3
GaspardD 1:8faddee0e52f 4 PwmOut pwm_ESC(PD_12);
GaspardD 1:8faddee0e52f 5 DigitalOut relay_ESC(PG_0);
GaspardD 8:f23601373e8b 6 E_STATE_ESC e_ESC_state;
GaspardD 6:ab9f3695633f 7 Timer t_emergency_brake;
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 8:f23601373e8b 46 rs_UTILS_pc.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 8:f23601373e8b 57 rs_UTILS_pc.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 6:ab9f3695633f 75 t_emergency_brake.start();
GaspardD 6:ab9f3695633f 76 t_emergency_brake.reset();
GaspardD 1:8faddee0e52f 77 break;
GaspardD 5:8bbe640528bc 78
GaspardD 1:8faddee0e52f 79 case ESC_COMMAND:
GaspardD 8:f23601373e8b 80 switch(e_SECTIONS_state) {
GaspardD 6:ab9f3695633f 81 case RUNNING_SECTION:
GaspardD 8:f23601373e8b 82 pwm_ESC.pulsewidth_us(s_UTILS_currentSection->targetSpeed_mps);
GaspardD 6:ab9f3695633f 83 break;
GaspardD 6:ab9f3695633f 84 case STOPPED:
GaspardD 6:ab9f3695633f 85 break;
GaspardD 6:ab9f3695633f 86 default:
GaspardD 6:ab9f3695633f 87 break;
GaspardD 8:f23601373e8b 88 }
GaspardD 1:8faddee0e52f 89 break;
GaspardD 1:8faddee0e52f 90 default:
GaspardD 5:8bbe640528bc 91 break;
GaspardD 1:8faddee0e52f 92 }
GaspardD 5:8bbe640528bc 93
GaspardD 1:8faddee0e52f 94 return;
GaspardD 1:8faddee0e52f 95 }