File content as of revision 14:914467165f34:

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


s_Section s_section1;
s_Section s_section2;
s_Section s_section2_1;
s_Section s_section3;
s_Section s_section3_1;
s_Section s_section4;
s_Section s_section5;
s_Section s_section5_1;
s_Section s_section6_0;
s_Section s_section6;
s_Section s_section6_1;
s_Section s_section7;
s_Section s_section8;
s_Section s_section9;
s_Section s_section9_1;
s_Section s_section10;
s_Section s_section11;
s_Section s_section11_1;
s_Section s_section12;

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.5;
    s_section1.targetSpeed_mps = 1750;
    s_section1.lng_section_m = 0.8;
    s_section1.coef_pos = 40;
    s_section1.coef_bord = 0.6;
    s_section1.plagePulseFreinage = 20;

    //ligne droite debut
    s_section2.nextSection = &s_section2_1;
    s_section2.consigne_position = 0.5;
    s_section2.targetSpeed_mps = 1770;
    s_section2.lng_section_m = 10.0;
    s_section2.coef_pos = 30;
    s_section2.coef_bord = 0.5;
    s_section2.plagePulseFreinage = 50;
    
    
    //ligne droite debut
    s_section2_1.nextSection = &s_section3;
    s_section2_1.consigne_position = 0.4;
    s_section2_1.targetSpeed_mps = 1600;
    s_section2_1.lng_section_m = 3.0;
    s_section2_1.coef_pos = 30;
    s_section2_1.coef_bord = 0.4;
    s_section2_1.plagePulseFreinage = 50;
    
    //epingle 1 debut
    s_section3.nextSection = &s_section3_1;
    s_section3.consigne_position = 0.7;
    s_section3.targetSpeed_mps = 1620;
    s_section3.lng_section_m = 2.0;
    s_section3.coef_pos = 30;
    s_section3.coef_bord = 2.0;
    s_section3.plagePulseFreinage = 20;
    
    //epingle 1 milieu
    s_section3_1.nextSection = &s_section4;
    s_section3_1.consigne_position = 0.7;
    s_section3_1.targetSpeed_mps = 1690;
    s_section3_1.lng_section_m = 4.0;
    s_section3_1.coef_pos = 30;
    s_section3_1.coef_bord = 2.0;
    s_section3_1.plagePulseFreinage = 20;
    
    //post epingle 1
    s_section4.nextSection = &s_section5;
    s_section4.consigne_position = 0.8;
    s_section4.targetSpeed_mps = 1720;
    s_section4.lng_section_m = 5.0;
    s_section4.coef_pos = 30;
    s_section4.coef_bord = 1.3;
    s_section4.plagePulseFreinage = 30;
    
    //droite epingle chicane
    s_section5.nextSection = &s_section5_1;
    s_section5.consigne_position = 0.6;
    s_section5.targetSpeed_mps = 1780;
    s_section5.lng_section_m = 7.0;
    s_section5.coef_pos = 30;
    s_section5.coef_bord = 0.7;
    s_section5.plagePulseFreinage = 40;
    
    
    //avant chicane
    s_section5_1.nextSection = &s_section6_0;
    s_section5_1.consigne_position = 0.8;
    s_section5_1.targetSpeed_mps = 1650;
    s_section5_1.lng_section_m = 4.0;
    s_section5_1.coef_pos = 30;
    s_section5_1.coef_bord = 2.0;
    s_section5_1.plagePulseFreinage = 40;
    
    //pre chicane
    s_section6_0.nextSection = &s_section6;
    s_section6_0.consigne_position = 0.5;
    s_section6_0.targetSpeed_mps = 1650;
    s_section6_0.lng_section_m = 3.0;
    s_section6_0.coef_pos = 30;//50
    s_section6_0.coef_bord = 1.0;//2.0
    s_section6_0.plagePulseFreinage = 45;
   
   
    //chicane
    s_section6.nextSection = &s_section7;
    s_section6.consigne_position = 0.58;
    s_section6.targetSpeed_mps = 1675;
    s_section6.lng_section_m = 17.0;
    s_section6.coef_pos = 30;//50
    s_section6.coef_bord = 2.0;//2.0
    s_section6.plagePulseFreinage = 50;
    
    //fin chicane SKIPPED
    s_section6_1.nextSection = &s_section7;
    s_section6_1.consigne_position = 0.58;
    s_section6_1.targetSpeed_mps = 1675;
    s_section6_1.lng_section_m = 0.0;
    s_section6_1.coef_pos = 30;//50
    s_section6_1.coef_bord = 2.0;//2.0
    s_section6_1.plagePulseFreinage = 50;
    
   //ligne droite postchicane
    s_section7.nextSection = &s_section8;
    s_section7.consigne_position = 0.4;
    s_section7.targetSpeed_mps = 1770;
    s_section7.lng_section_m = 8.0;
    s_section7.coef_pos = 30;
    s_section7.coef_bord = 0.5;
    s_section7.plagePulseFreinage = 50;
    
    
    //freinage avant epingle
    s_section8.nextSection = &s_section9;
    s_section8.consigne_position = 0.4;
    s_section8.targetSpeed_mps = 1620;
    s_section8.lng_section_m = 4.0;
    s_section8.coef_pos = 30;
    s_section8.coef_bord = 0.4;
    s_section8.plagePulseFreinage = 50;
    
    //epingle 2 debut
    s_section9.nextSection = &s_section9_1;
    s_section9.consigne_position = 0.7;
    s_section9.targetSpeed_mps = 1620;
    s_section9.lng_section_m = 2.0;
    s_section9.coef_pos = 30;
    s_section9.coef_bord = 2.0;
    s_section9.plagePulseFreinage = 20;
    
    //epingle 2 milieu
    s_section9_1.nextSection = &s_section10;
    s_section9_1.consigne_position = 0.7;
    s_section9_1.targetSpeed_mps = 1670;
    s_section9_1.lng_section_m = 4.0;
    s_section9_1.coef_pos = 30;
    s_section9_1.coef_bord = 2.0;
    s_section9_1.plagePulseFreinage = 20;
    
    //post epingle 2
    s_section10.nextSection = &s_section11;
    s_section10.consigne_position = 0.8;
    s_section10.targetSpeed_mps = 1720;
    s_section10.lng_section_m = 5.0;
    s_section10.coef_pos = 30;
    s_section10.coef_bord = 1.3;
    s_section10.plagePulseFreinage = 30;
    
    //dernière ligne droite
    s_section11.nextSection = &s_section11_1;
    s_section11.consigne_position = 0.65;
    s_section11.targetSpeed_mps = 1750;
    s_section11.lng_section_m = 8.0;
    s_section11.coef_pos = 20;
    s_section11.coef_bord = 0.4;
    s_section11.plagePulseFreinage = 40;
    
    //dernière ligne droite
    s_section11_1.nextSection = &s_section12;
    s_section11_1.consigne_position = 0.5;
    s_section11_1.targetSpeed_mps = 1810;
    s_section11_1.lng_section_m = 30.0;
    s_section11_1.coef_pos = 20;
    s_section11_1.coef_bord = 0.4;
    s_section11_1.plagePulseFreinage = 100;
  
    
    //frein
    s_section12.nextSection = NULL;
    s_section12.consigne_position = 0.5;
    s_section12.targetSpeed_mps = 1300;
    s_section12.lng_section_m = 0.1;
    s_section12.coef_pos = 80;
    s_section12.coef_bord = 1.0;
    s_section12.plagePulseFreinage = 20;

    return;
}
void update_sm_sections()
{

    E_STATE_SECTIONS e_next_state = e_SECTIONS_state;

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

    if(b_UTILS_flag_emergency_stop) {
        rs_LOG.printf("emergency stop -> STOPPED\r\n");
        e_next_state = STOPPED;
    } else {
        switch (e_SECTIONS_state) {
            case INIT_SECTION:
                e_next_state = RUNNING_SECTION;
                //rs_LOG_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_LOG_pc.printf("LOADING SECTION !\r\n");
                }
                break;
            case LOADING_SECTION:
                if(s_UTILS_currentSection != NULL) {
                    e_next_state = RUNNING_SECTION;
                    //rs_LOG_pc.printf("RUNNING SECTION !\r\n");
                } else {
                    b_UTILS_flag_emergency_stop = true;
                    log_stop();
                    e_next_state = STOPPED;
                }
                break;
            case STOPPED:
                if(b_UTILS_flag_button_SECTIONS) {
                    e_next_state = INIT_SECTION;
                    rs_LOG.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_LOG.printf("sect 1 INITIALIZED\r\n");
        case RUNNING_SECTION:
            break;
        case LOADING_SECTION:
            //rs_LOG_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;
}