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

Committer:
GaspardD
Date:
2019-09-29
Revision:
7:23bb20892c8c
Parent:
6:ab9f3695633f
Child:
8:f23601373e8b

File content as of revision 7:23bb20892c8c:

#include "sm_esc.h"
#include "sm_sections.h"

PwmOut pwm_ESC(PD_12);
DigitalOut relay_ESC(PG_0);
E_STATE_ESC e_stateESC;
Timer t_emergency_brake;
int pulsewidth_test = 1650;

void init_sm_esc()
{
    e_stateESC = ESC_DISABLED;
    return;
}

void update_sm_esc()
{
    E_STATE_ESC next_state = e_stateESC;

    switch(e_stateESC) {
        case ESC_DISABLED:
            if(b_UTILS_flag_button && !b_UTILS_flag_emergency_stop) {
                next_state = ESC_INIT;
            }
            break;
        case ESC_INIT:
            next_state = ESC_COMMAND;
            break;
        case ESC_COMMAND:
            if(b_UTILS_flag_emergency_stop) {
                next_state = ESC_DISABLED;
            }
            break;
        default:
            break;
    }

    e_stateESC = next_state;
    return;
}

void output_sm_esc()
{
    switch(e_stateESC) {
        case ESC_DISABLED:
            if(b_UTILS_flag_emergency_stop) {
                pc.printf("EMERGENCY STOP\r\n");
                b_UTILS_flag_emergency_stop = false;
                if(b_chassis_with_pushbutton) {
                    relay_ESC = 1;
                    wait(0.5);
                }
                relay_ESC = 0;
            }
            break;

        case ESC_INIT:
            pc.printf("Init esc\r\n");
            odroid.printf("pulsewidth_test: %d\r\n",pulsewidth_test);
            b_UTILS_flag_button = false;
            pwm_ESC.period_ms(ESC_PERIOD_DURATION_MS);          //20 ms is default
            pwm_ESC.pulsewidth_us(0);

            relay_ESC = 1;
            if(b_chassis_with_pushbutton) {
                wait(0.5);
                relay_ESC = 0;
            }
            wait(0.1);

            pwm_ESC.pulsewidth_us(ESC_PULSE_MIN_US);
            wait(3);
            pwm_ESC.pulsewidth_us(ESC_PULSE_MAX_US);
            wait(1);
            pwm_ESC.pulsewidth_us(ESC_PULSE_IDLE_US);
            wait(1);
            t_emergency_brake.start();
            t_emergency_brake.reset();
            break;

        case ESC_COMMAND:
            if(t_emergency_brake.read_ms() < 2000) {
                pwm_ESC.pulsewidth_us(pulsewidth_test);
            } else {
                pc.printf("emergency stop flag raised\r\n");
                b_UTILS_flag_emergency_stop = true;
                t_emergency_brake.stop();
                t_emergency_brake.reset();
            }
            /*switch(e_stateSections) {
                case RUNNING_SECTION:
                    pwm_ESC.pulsewidth_us(1660);
                    break;
                case STOPPED:
                    pc.printf("emergency stop flag raised\r\n");
                    b_UTILS_flag_emergency_stop = true;
                    break;
                default:
                    break;
            }*/
            break;
        default:
            break;
    }

    return;
}