File content as of revision 8:f23601373e8b:

#include "sm_sections.h"
#include "sm_mpu.h"


s_Section s_section1;
s_Section s_section2;

E_STATE_SECTIONS e_SECTIONS_state;


void init_sm_sections()
{

    d_ODOM_distFromSectionStart_m = 0.0;
    e_SECTIONS_state = STOPPED;
    s_section1.nextSection = NULL;


//section de départ
    s_section1.nextSection = &s_section2;
    s_section1.consigne_position = 0.75;
    s_section1.targetSpeed_mps = 1640;
    s_section1.lng_section_m = 4.0;
    s_section1.coef_p = 1.0;
    s_section1.coef_d = 0.0;
    s_section1.coef_i = 0.000;

    //epingle 1
    s_section2.nextSection = NULL;
    s_section2.consigne_position = 0.75;
    s_section2.targetSpeed_mps = 1640;
    s_section2.lng_section_m = 4.0;
    s_section2.coef_p = 1.0;
    s_section2.coef_d = 0.0;
    s_section2.coef_i = 0.000;

    return;
}
void update_sm_sections()
{

    E_STATE_SECTIONS e_next_state = e_SECTIONS_state;

    //rs_UTILS_pc.printf("\r\nUpdate Section\r\n");

    if(b_UTILS_flag_emergency_stop) {
        rs_UTILS_pc.printf("emergency stop -> STOPPED\r\n");
        e_next_state = STOPPED;
    } else {
        switch (e_SECTIONS_state) {
            case INIT_SECTION:
                e_next_state = RUNNING_SECTION;
                //rs_UTILS_pc.printf("RUNNING SECTION\r\n");
                break;
            case RUNNING_SECTION:
                if(d_ODOM_distFromSectionStart_m > s_UTILS_currentSection->lng_section_m) {
                    e_next_state = LOADING_SECTION;
                    //rs_UTILS_pc.printf("LOADING SECTION !\r\n");
                }
                break;
            case LOADING_SECTION:
                if(s_UTILS_currentSection != NULL) {
                    e_next_state = RUNNING_SECTION;
                    //rs_UTILS_pc.printf("RUNNING SECTION !\r\n");
                } else {
                    b_UTILS_flag_emergency_stop = true;
                    e_next_state = STOPPED;
                }
                break;
            case STOPPED:
                if(b_UTILS_flag_button_SECTIONS) {
                    e_next_state = INIT_SECTION;
                    rs_UTILS_pc.printf("INIT SECTION !\r\n");
                }
                break;
            default:
                break;
        }
    }
    e_SECTIONS_state = e_next_state;
    return;
}
void output_sm_sections()
{
    switch (e_SECTIONS_state) {
        case INIT_SECTION:
            b_UTILS_flag_button_SECTIONS = false;
            s_UTILS_currentSection = &s_section1;
            rs_UTILS_pc.printf("sect 1 INITIALIZED\r\n");
             mpu_log_start(10000);
        case RUNNING_SECTION:
            break;
        case LOADING_SECTION:
            //rs_UTILS_pc.printf("next section loading\r\n");
            s_UTILS_currentSection=s_UTILS_currentSection->nextSection;
            d_ODOM_distFromSectionStart_m = 0.0;
            break;
        case STOPPED:
            //on est à l'arret
            break;
        default:
            break;
    }
    return;
}