File content as of revision 51:09ecba68d0cf:

#include "stateMachines.h"
//#if DEBUG >0
Serial pc(USBTX, USBRX);    // tx, rx
//#endif

//*************** declarations ***************

//time monitoring
#ifdef DUMP_SAMPLIG_PERIOD
Timer timerLog;
#endif

Timer timeSinceStart;// temps.start()/stop()/sec: read()/ms: read_ms()/µs: read_us()
Timer timerSinceTachy;
Timer timersinceSerial;

double distMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne
double distMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit pour moyenne
double shortDistMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche petites distances pour moyenne
double shortDistMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit petites distances pour moyenne
double distMurG45[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 45deg pour moyenne
double distMurD45[NB_ECHANTILLONS_IR];//buffer tournant ir avant droit 45deg pour moyenne
#ifdef DLVV
double distMurG10[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 10deg pour moyenne
double distMurD10[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit 10deg pour moyenne
double distMurFront[NB_ECHANTILLONS_IR];//buffer tournant ir front
#endif
double distMurG90Moy;
double distMurD90Moy;
double shortDistMurG90Moy;
double shortDistMurD90Moy;
double trueDistMurG90Moy;
double trueDistMurD90Moy;
double distMurG45Moy;
double distMurD45Moy;

#ifdef DLVV
double distMurG10Moy;
double distMurD10Moy;
double distMurFrontMoy
#endif
int index_fifo_ir = 0;//pour géreer le buffer tournant
int index_fifo_lidar = 0;
//sections
s_Section p_section1;
s_Section p_section2;
s_Section p_section3;

//PWM Controls
PwmOut      PwmMotor(PB_6);     // PWM4 ch1 TIM4
PwmOut      PwmDirection(PB_5); // PWM3 ch2 TIM3

int pulseDirection_us = DIRECTION_PULSE_MIDDLE;
double pulseDirection_us_temp ;
int pulseSpeed_us = INITAL_PULSE_SPEED_US;
//Capteurs direction
AnalogIn anaG90(CAPT_90_GAUCHE);//capteur ir coté gauche
AnalogIn anaD90(CAPT_90_DROITE);//capteur ir coté droit
AnalogIn anaShortG90(CAPT_90_GAUCHE_SHORT);//capteur ir coté gauche short
AnalogIn anaShortD90(CAPT_90_DROITE_SHORT);//capteur ir coté droit short
AnalogIn anaG45(CAPT_45_GAUCHE);//capteur ir avant gauche 45 deg
AnalogIn anaD45(CAPT_45_DROITE);//capteur ir avant droit 45deg
#ifdef DLVV
AnalogIn anaDlvvG(CAPT_10_GAUCHE);//capteur ir avant droit 10 deg
AnalogIn anaDlvvD(CAPT_10_DROITE);//capteur ir coté droit 10 deg
AnalogIn anaDlvvFront(CAPT_DEVANT);//capteur ir avant
#endif

//piste
double largeurPiste90 = 150.0;
double largeurPiste45 = 150.0;
double positionSurPiste90 = 75.0;
double positionSurPiste90Prev = positionSurPiste90;
double positionSurPiste45 = 75.0;
double positionSurPiste45Prev = positionSurPiste45;

double derive45,derive90;
int lastDifferences90[NB_INTEGRAL_SAMPLES] = {0};//for integral correction
int lastDifferenceIndex = 0;
int integralSum;

//Capteur vitesse
InterruptIn it_tachymeter(PA_11);


//LIDAR
Serial serialLidar(PC_10,PC_11);  // tx, rx

int distLidar;// LiDAR actually measured distance value
int distLidarPrev;
int strengthLidar;// LiDAR signal strength
int strengthLidarPrev;
int check;// check numerical value storage
int i;
int uart[9];// store data measured by LiDAR
const int HEADER=0x59;// data package frame header

//SPEED
double maxSpeed_cmps = 0;
double tachySpeed_cmps = 0; //en cm/s
double tachyStepsRegister = 0;
double tachySectionDist_cm = 0;
double tachyTotalDist_cm = 0.0;

#ifdef FREINAGE_ADAPTATIF
Timer brakingTimer;
int brakingDurationNeeded_us = 0;
#endif

//Etats
MUR_ST st_murs;
MUR_ST st_tmpMurs;
SECTION_ST st_currentSection;
SECTION_ST st_tmpSection;
MAX_SPEED_ST st_maxSpeed;
MAX_SPEED_ST st_tmpMaxSpeed;
THROTTLE_ST st_thro;
THROTTLE_ST st_tmpThro;
#ifdef DLVV
OBSTACLE_ST st_obstacle;
#endif

s_Section* p_sectionCourante = NULL;
bool dumped = false;


// +++++++++++++++++++++++++++++++++++++++++++ SAMPLING +++++++++++++++++++++++++++++++++++++++++++
#ifdef SAMPLING
s_Sample history[TAILLE_SAMPLES];
int indexSample = 0;
void initSamples(void)
{
    for(int m=0; m<TAILLE_SAMPLES; m++) {
        history[m].states.murs_dlvv = '0';
        history[m].states.section = '0';
        history[m].states.maxSpeed = '0';
        history[m].states.throttle = '0';
        history[m].time = 0;
        history[m].position45 = 0.0;
        history[m].position90 = 0.0;
        history[m].largeurPiste = 0.0;
        history[m].dist = 0.0;
        history[m].pwm_thro_us = 0;
        history[m].pwm_dir_us = 0;
        history[m].distLidar = 0;
        history[m].strLidar = 0;
    }
#if DEBUG > 0
    pc.printf("[INIT SAMPLE DONE]\r\n");
#endif
    return;
}
void sampleLog(void)
{
#ifdef DUMP_SAMPLIG_PERIOD
    if(timerLog.read_us() > DUMP_SAMPLIG_PERIOD) {
        timerLog.reset();
        timerLog.start();
#endif
        if(indexSample < TAILLE_SAMPLES) {
#ifdef DLVV
            history[indexSample].states.murs_dlvv = (char)st_obstacle;
#else
            history[indexSample].states.murs_dlvv = (char)st_murs;
#endif
            history[indexSample].states.section = (char)st_currentSection;
            history[indexSample].states.maxSpeed = (char)st_maxSpeed;
            history[indexSample].states.throttle = (char)st_thro;
            history[indexSample].time = timeSinceStart.read_us() ;
            history[indexSample].position45 = positionSurPiste45;
            history[indexSample].position90 = positionSurPiste90;
            history[indexSample].largeurPiste = largeurPiste90;
            history[indexSample].pwm_thro_us = pulseSpeed_us;
            history[indexSample].pwm_dir_us = pulseDirection_us;
            history[indexSample].dist = tachySectionDist_cm,
                                 history[indexSample].distLidar = distLidar;
            history[indexSample].strLidar = strengthLidar;
            indexSample++;
#if DEBUG > 0
            pc.printf("\r\nodo:%d  dist = %.4lf    \tstrength = %.4ld    \tC45D: %.4lf C45G: %.4lf  C90D: %.4lf  C90G: %.4lf  looptime: %.4lf micros",tachySectionDist_cm,distLidar,strengthLidar,distMurD45Moy,distMurG45Moy,distMurD90Moy,distMurG90Moy,timeSinceStart.read_us());// output signal strength value
            pc.printf("\r\nstate Murs: %.4lf, state Section %.4lf, state MaxSpeed %.4lf, state Throttle %.4lf\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro);
            //wait(2);
            timeSinceStart.reset();
            timeSinceStart.start();
#endif
        }
        return;
#ifdef DUMP_SAMPLIG_PERIOD
    }
#endif
}
void transmitData(void)
{
#if DEBUG > 0
    pc.printf("[START TO TRANSMIT]\r\n");
#endif
    serialLidar.printf("time,position 45,position 90,largeur piste,pwm_thro_us,pwm_dir_us,dist,murs/dlvv,section,maxSpeed,throttle\r\n");
    for(int p=0; p<indexSample; p++) {
        serialLidar.printf("%d,%.5f,%.5f,%.5f,%d,%d,%.5f,%d,%d,%d,%d\r\n",
                           history[p].time,
                           history[p].position45,
                           history[p].position90,
                           history[p].largeurPiste,
                           history[p].pwm_thro_us,
                           history[p].pwm_dir_us,
                           history[p].dist,
                           history[p].states.murs_dlvv,
                           history[p].states.section,
                           history[p].states.maxSpeed,
                           history[p].states.throttle);
    }
    return;
}

#endif

// +++++++++++++++++++++++++++++++++++++++++++ FUNCTION UTILS +++++++++++++++++++++++++++++++++++++++++++

#if DEBUG >= -1
void pressed(void)
{
#if DEBUG > 0
    pc.printf("[BTN PRESSED]\r\n");
#endif
if(!dumped)
{
    transmitData();
    dumped = true;
}else{
    initSamples();
    p_sectionCourante = &p_section1;
    dumped = false;
    }
}
#endif

void initIntegrationTable()
{
    for(int h=0; h<NB_INTEGRAL_SAMPLES; h++) {
        lastDifferences90[h] = 0;
    }
    return;
}


double getShortDistMoy(AnalogIn* p,double* tab,int size)
{
    tab[index_fifo_ir] = 3.3 * (double)p->read(); // on convertit directement en volts
    //tension proportionelle à l'inverse de la distance en dessous de 2V
    if(tab[index_fifo_ir]< 0.1034) {
        tab[index_fifo_ir]= 0.11;
    }
    tab[index_fifo_ir] = (11.3531/(tab[index_fifo_ir]-0.1034))-0.42;

    int sumMoy = 0;
    for(int k=0; k<size; k++) {
        sumMoy+=tab[k];
    }
    return sumMoy/size;
}

double getDistMoy(AnalogIn* p,double* tab,int size)
{
    double distcapteur = 0;
    distcapteur =  1.0/(0.0161  * (double)p->read()); // on convertit directement en volts
    //distcapteur =  1.0/(0.02  * (double)p->read()); // on convertit directement en volts
    //tension proportionelle à l'inverse de la distance
    //tab[index_fifo_ir] = 1.0/(0.0161 * (double)tab[index_fifo_ir]);
    //int sumMoy = 0;
    //for(int k=0; k<size; k++) {
    //    sumMoy+=tab[k];
    //}
    return distcapteur;
}
void tachyCheck()
{
    if(timerSinceTachy.read_us() > 500000) {
        tachySpeed_cmps = 0.0;
    }
    return;
}


void it4cm()
{
    //le timer sert de flag sur le freinage:
    //si le biniou ne bouge plus, il n'essaye plus de freiner tant qu'il n'y a pas un nouveau calcul de la vitesse
    //dans la fonction tachyCheck() >> quand il n'y a pas eu d'IT en 500 ms si la vitesse est inférieure à 0.16 m/s, elle sera considérée comme nulle.

    tachySectionDist_cm += TACHY_CM;
    tachyTotalDist_cm += TACHY_CM;
    tachySpeed_cmps = (TACHY_CM * 1000000.0)/(double)timerSinceTachy.read_us(); // a chaque IT on a parcouru 8 cm soit (8*1000000)/durée
#if DEBUG > 2
    pc.printf("IT: distance parcourue %.4lf       , vitesse:%.4lf         \r\n",tachyTotalDist_cm,tachySpeed_cmps);
#endif
    timerSinceTachy.reset();
    timerSinceTachy.start();
    return;

#if DEBUG > 0
    pc.printf("IT tachy\r\n");
#endif
}

void it_serial()
{
    if(serialLidar.getc()==HEADER) { // determine data package frame header 0x59
        uart[0]=HEADER;
        if(serialLidar.getc()==HEADER) { //determine data package frame header 0x59
            uart[1]=HEADER;
            for(i=2; i<9; i++) { // store data to array
                uart[i]=serialLidar.getc();
            }
            check=uart[0]+uart[1]+uart[2]+uart[3]+uart[4]+uart[5]+uart[6]+uart[7];
            if(uart[8]==(check&0xff)) { // check the received data as per protocols
                distLidarPrev = distLidar;
                distLidar=uart[2]+uart[3]*256;// calculate distance value
                strengthLidarPrev = strengthLidar;
                strengthLidar=uart[4]+uart[5]*256;// calculate signal strength value
                timersinceSerial.reset();
                timersinceSerial.start();
            }
        }
    }
}
// +++++++++++++++++++++++++++++++++++++++++++ STATES MACHINES +++++++++++++++++++++++++++++++++++++++++++

//########## INIT STATES MACHINES ##########
void mursInit(void)
{
#if DEBUG > 0
    pc.printf("Init Murs\r\n");
#endif
    timeSinceStart.start();
    st_murs=REF_BIDIR;
    PwmDirection.period_us(SPEED_PERIOD_US);
    PwmDirection.pulsewidth_us(DIRECTION_PULSE_MIDDLE);// milieu
    return;
}
#ifdef DLVV
void obstacleInit(void)
{
#if DEBUG > 0
    pc.printf("Init Obstacle\r\n");
#endif
    st_speedLimit=ALL_CLEAR;
    return;
}
#endif
void sectionInit(void)
{
#if DEBUG > 0
    pc.printf("Init Section\r\n");
#endif
    st_currentSection=ARRET;
    p_sectionCourante=&p_section1;
    it_tachymeter.fall(&it4cm);
    timerSinceTachy.start();
    tachySectionDist_cm = 0;
    tachyStepsRegister = 0;

    //section de test 1
    p_section1.nextSection =&p_section3;// &p_section2;
    p_section1.consigne_position = 75.0;
    p_section1.targetSpeed_cmps = 400.0;
    p_section1.slowSpeed_cmps = 328.0;
    p_section1.coef_p_speed = 1;
    p_section1.lidarWarningDist_cm = 120.0;
    p_section1.lng_section_cm = 1000.0;//10m
    p_section1.coef_p90 = 4.0;
    p_section1.coef_d90 = 0.0;
    p_section1.coef_i90 = 0.0;
    p_section1.coef_p45 = 0.0;
    p_section1.coef_d45 = 0.0;
    p_section1.coef_i45 = 0.0;

    //section de test
    p_section2.nextSection = &p_section3;
    p_section2.consigne_position = 75.0;
    p_section2.targetSpeed_cmps = 328.0;
    p_section2.slowSpeed_cmps = 328.0;
    p_section2.coef_p_speed = 1.0;
    p_section2.lidarWarningDist_cm = 300.0;
    p_section2.lng_section_cm = 200.0;//2m
    p_section2.coef_p90 = 1.0;
    p_section2.coef_d90 = 0.0;
    p_section2.coef_i90 = 0.0;
    p_section2.coef_p45 = 1.0;
    p_section2.coef_d45 = 0.0;
    p_section2.coef_i45 = 0.0;

    p_section3.nextSection = NULL;
    p_section3.consigne_position = 75.0;
    p_section3.targetSpeed_cmps = 0.0;
    p_section3.slowSpeed_cmps = 0.0;
    p_section3.coef_p_speed = 1.0;
    p_section3.lidarWarningDist_cm = 300.0;
    p_section3.lng_section_cm = 200.0;//2m
    p_section3.coef_p90 = 1.0;
    p_section3.coef_d90 = 0.0;
    p_section3.coef_i90 = 0.0;
    p_section3.coef_p45 = 1.0;
    p_section3.coef_d45 = 0.0;
    p_section3.coef_i45 = 0.0;
    
    return;
}

void maxSpeedInit(void)
{
#if DEBUG > 0
    pc.printf("Init Max Speed\r\n");
#endif
    st_maxSpeed=SPEED_MAX;
    maxSpeed_cmps= p_sectionCourante->targetSpeed_cmps;
    serialLidar.baud(115200);
    serialLidar.attach(&it_serial);
    return;
}

void throttleInit(void)
{
#if DEBUG > 0
    pc.printf("Init Throttle\r\n");
#endif
    st_thro = REGULATION_SPEED;
    PwmMotor.period_us(DIRECTION_PERIOD_MS);          //20 ms is default
    PwmMotor.pulsewidth_us(1000);//MIN
    wait(3);
    PwmMotor.pulsewidth_us(2000);//MAX
    wait(1);
    PwmMotor.pulsewidth_us(1500);//ZEROING
    wait(1);
    pulseSpeed_us = INITAL_PULSE_SPEED_US;
#if DEBUG > 0
    pc.printf("temps init: %.4lf micros\r\n",timeSinceStart.read_us());
    pc.printf("\r\nStates INIT: state Murs: %.4lf, state Section %.4lf, state MaxSpeed %.4lf, state Throttle %.4lf\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro);
    timeSinceStart.reset();
    timeSinceStart.start();
#endif
#ifdef DUMP_SAMPLIG_PERIOD
    timerLog.start();
#endif

    return;
}


//########## UPDATE STATES ##########
void mursUpdate(void)
{
#if (DEBUG > 3)
    pc.printf("\r\nUpdate MURS\r\n");
#endif
    //lectures

    distMurG45Moy = getDistMoy(&anaG45,distMurG45,NB_ECHANTILLONS_IR);
    distMurD45Moy = getDistMoy(&anaD45,distMurD45,NB_ECHANTILLONS_IR);
    distMurG90Moy = getDistMoy(&anaG90,distMurG90,NB_ECHANTILLONS_IR);
    distMurD90Moy = getDistMoy(&anaD90,distMurD90,NB_ECHANTILLONS_IR);
    shortDistMurG90Moy = getShortDistMoy(&anaShortG90,shortDistMurD90,NB_ECHANTILLONS_IR);
    shortDistMurD90Moy = getShortDistMoy(&anaShortD90,shortDistMurD90,NB_ECHANTILLONS_IR);
    index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR;


    trueDistMurD90Moy = distMurD90Moy;
    trueDistMurG90Moy = distMurG90Moy;

    if(shortDistMurG90Moy < DIST_MIN_LONG_CM) {
        trueDistMurG90Moy = shortDistMurG90Moy;
    } else {
        trueDistMurG90Moy = distMurG90Moy;
    }
    if(shortDistMurD90Moy < DIST_MIN_LONG_CM) {
        trueDistMurD90Moy = shortDistMurD90Moy;
    } else {
        trueDistMurD90Moy = distMurD90Moy;
    }


#ifdef DLVV
    switch (st_obstacle) {
        case FRONT_OBSTRUCTED:
            st_tmpMurs = REF_A_GAUCHE;
            return;
        case RIGHT_OBSTRUCTED:
            st_tmpMurs = REF_A_GAUCHE;
            return;
        case LEFT_OBSTRUCTED:
            st_tmpMurs = REF_A_DROITE;
            return;
        default:
            break;
    }

#endif
    st_tmpMurs = REF_BIDIR;

    st_murs = st_tmpMurs;

    return;
}
#ifdef DLVV
void obstacleUpdate(void)
{
    return;
}
#endif

void sectionUpdate(void)
{
#if (DEBUG > 3)
    pc.printf("\r\nUpdate Section\r\n");
#endif

    if(p_sectionCourante == NULL) {
        st_tmpSection = ARRET;
    } else {

        switch (st_currentSection) {
            case RUNNING_SECTION:
                if(tachySectionDist_cm > p_sectionCourante->lng_section_cm) { //on pourrait rajouter un test lidar
                    st_tmpSection = LOADING_SECTION;
                } else {
                    return;
                }
                break;
            case LOADING_SECTION:
                if(p_sectionCourante != NULL) { //la section a ete chargee dans sectionOutput
                    st_tmpSection = RUNNING_SECTION;
                } else {
                    st_tmpSection=ARRET;
                }
                break;
            case ARRET:
                if(p_sectionCourante != NULL) {
                    st_tmpSection = RUNNING_SECTION;
                } else {
                    return;
                }
                break;
            default:
                break;
        }
    }
    st_currentSection = st_tmpSection;
    return;
}

void maxSpeedUpdate(void)
{
#if (DEBUG > 3)
    pc.printf("\r\nUpdate MaxSpeed\r\n");
#endif
    if(p_sectionCourante == NULL) {
        st_tmpMaxSpeed = BLOCKED;
    } else if(st_maxSpeed == BLOCKED && strengthLidar > LIDAR_STRENGTH_THRESOLD && distLidar < p_sectionCourante->lidarWarningDist_cm) {
        st_tmpMaxSpeed = BLOCKED;
    } else if( strengthLidar > LIDAR_STRENGTH_THRESOLD ) {
        if( distLidar > p_sectionCourante->lidarWarningDist_cm ) {
            st_tmpMaxSpeed = SPEED_MAX;
        } else if(  strengthLidar > LIDAR_STRENGTH_THRESOLD &&
                    strengthLidarPrev > LIDAR_STRENGTH_THRESOLD &&
                    distLidar < p_sectionCourante->lidarWarningDist_cm &&
                    distLidarPrev-distLidar > 3) {
            st_tmpMaxSpeed = BLOCKED;
        } else {
            st_tmpMaxSpeed = SPEED_WARNING;
        }
    } else {
        st_tmpMaxSpeed = SPEED_MAX;
    }

    st_maxSpeed = st_tmpMaxSpeed;
    return;
}

void throttleUpdate(void)
{
#if (DEBUG > 3)
    pc.printf("\r\nUpdate Throttle\r\n");
#endif
    switch (st_thro) {
        case REGULATION_SPEED:
            if( st_currentSection == ARRET ||
                    st_maxSpeed == BLOCKED ) {
                st_tmpThro = BRAKING;
            }
#ifdef FREINAGE_ADAPTATIF
            else if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS)) {
                st_tmpThro = BRAKING;
                brakingDurationNeeded_us = 10000.0 * MASSE_BINIOU_KG *((tachySpeed_cmps-maxSpeed_cmps)*(tachySpeed_cmps+maxSpeed_cmps)) / (PUISSANCE_FREINAGE_W *2) ;//vitesse en m/s et temps en us
                brakingTimer.reset();
                brakingTimer.start();
            }
#endif
            else {
                return;
            }
            break;
        case BRAKING:
            if(st_maxSpeed == BLOCKED && timersinceSerial.read_us() > 200000) {
                p_sectionCourante= NULL;
                st_tmpThro = STOPPED;
            } else if( st_maxSpeed == BLOCKED
#ifdef FREINAGE_ADAPTATIF
                       || brakingTimer.read_us() < brakingDurationNeeded_us
#endif
                     ) {
                st_tmpThro = BRAKING;
            } else if(st_currentSection == ARRET) {
                st_tmpThro = STOPPED;
            } else {
#ifdef FREINAGE_ADAPTATIF
                brakingDurationNeeded_us = 0;
#endif
                st_tmpThro = REGULATION_SPEED;
            }
            break;
        case STOPPED:
            if(st_currentSection == RUNNING_SECTION) {
                st_tmpThro = REGULATION_SPEED;
            } else {
                st_tmpThro = STOPPED;
            }
            break;
        default:
            break;
    }
    st_thro = st_tmpThro;
    return;
}

//########## OUTPUT STATES ##########
//updating output parameters
void mursOutput(void)
{
    if(p_sectionCourante != NULL) {
        //pour dériver
        positionSurPiste45Prev = positionSurPiste45;
        positionSurPiste90Prev = positionSurPiste90;
#if (DEBUG > 3)
        pc.printf("\r\n Output MURS\r\n");
#endif

        switch (st_murs) {
            case REF_A_GAUCHE://par defaut, on compte à partir de la bordure gauche
                largeurPiste90 = 150.0;
                positionSurPiste90 = ( trueDistMurG90Moy + DEMI_LARGEUR_BINIOU_CM );
                positionSurPiste45 = ( distMurG45Moy * 1.414214/2.0 ) + DEMI_LARGEUR_BINIOU_CM;
#ifdef OMAR
                pc.printf("REF_A_GAUCHE\r\n");
#endif
                break;
            case REF_A_DROITE://par defaut, on compte à partir de la bordure gauche
                largeurPiste90 = 150.0;
                positionSurPiste90 = largeurPiste90 - ( DEMI_LARGEUR_BINIOU_CM + trueDistMurD90Moy ) ;
                positionSurPiste45 = largeurPiste90 - ( distMurD45Moy * (1.414214)/2.0 ) + DEMI_LARGEUR_BINIOU_CM;
#ifdef OMAR
                pc.printf("REF_A_DROITE\r\n");
#endif
                break;
            default://REF_BIDIR
                largeurPiste90 = trueDistMurG90Moy + trueDistMurD90Moy ;
                largeurPiste45 = distMurG45Moy + distMurD45Moy ;
                positionSurPiste90 = (trueDistMurG90Moy );
                positionSurPiste45 = (distMurG45Moy);
#ifdef OMAR
                pc.printf("REF_BIDIR\r\n");
#endif
                break;
        }


        //deriv correction
        derive45 = positionSurPiste45 - positionSurPiste45Prev;
        derive90 = positionSurPiste90 - positionSurPiste90Prev;

#ifdef OMAR/*
        pc.printf("derive45 => %.4lf \r\n  ",derive45);
        pc.printf("derive90 => %.4lf \r\n  ",derive90);
        pc.printf("distMurG45Moy => %.4lf \r\n  ",distMurG45Moy);
        pc.printf("distMurD45Moy => %.4lf \r\n  ",distMurD45Moy);
        pc.printf("trueDistMurG90Moy => %.4lf \r\n  ",trueDistMurG90Moy);
        pc.printf("trueDistMurD90Moy => %.4lf \r\n  ",trueDistMurD90Moy);
        pc.printf("positionSurPiste90 => %.4lf \r\n  ",positionSurPiste90);
        pc.printf("positionSurPiste45 => %.4lf \r\n  ",positionSurPiste45);
        pc.printf("largeurPiste90 => %.4lf \r\n  ",largeurPiste90);*/
#endif

        //integral correction
        lastDifferences90[lastDifferenceIndex] = (p_sectionCourante->consigne_position - positionSurPiste90);
        integralSum=0;
        for(int f=0; f<NB_INTEGRAL_SAMPLES; f++) {
            integralSum+=lastDifferences90[f];
        }
        lastDifferenceIndex = (lastDifferenceIndex + 1)%NB_INTEGRAL_SAMPLES;




        //application des coefficients

        pulseDirection_us = (int)(
                                ((positionSurPiste45 - (largeurPiste45/2)) * p_sectionCourante->coef_p45)
                                + ((positionSurPiste90 - (largeurPiste90/2)) * p_sectionCourante->coef_p90)
                                + ( -derive45                                  * p_sectionCourante->coef_d45)
                                + ( -derive90                                  * p_sectionCourante->coef_d90)
                                + ( -integralSum                               * p_sectionCourante->coef_i90)
                            )//(300.0/(tachySpeed_cmps+1.0)) // faire du test reel pour afiné ou un calcul
                            + DIRECTION_PULSE_MIDDLE;


#ifdef OMAR
        pc.printf("p_sectionCourante->consigne_position => %.4lf \r\n  ",p_sectionCourante->consigne_position);
        pc.printf("positionSurPiste45 => %.4lf \r\n  ",positionSurPiste45);
        pc.printf("p_sectionCourante->coef_p => %.4lf \r\n  ",p_sectionCourante->coef_p);
#endif
//gestioon du dépassement
        if(pulseDirection_us > DIRECTION_PULSE_MAX) { //POUR TOURNER A GAUCHE
#if DEBUG > 1
            pc.printf("!!! OVER PWM Direction pulse: %.4lf\r\n",pulseDirection_us);
#endif
            pulseDirection_us = DIRECTION_PULSE_MAX;
        } else if(pulseDirection_us < DIRECTION_PULSE_MIN ) { //POUR TOURNER A DROITE
#if DEBUG > 1
            pc.printf("!!! UNDER PWM Direction pulse: %.4lf\r\n",pulseDirection_us);
#endif
            pulseDirection_us = DIRECTION_PULSE_MIN ;
        }
#if DEBUG > 1
        pc.printf("PWM Direction pulse: %.4lf\r\n",pulseDirection_us);
#endif
        PwmDirection.pulsewidth_us(pulseDirection_us);
        return;
    }
}

#ifdef DLVV
void obstacleOutput(void)
{
    return;
}
#endif
void sectionOutput(void)
{
#if (DEBUG > 3)
    pc.printf("\r\n Output Section\r\n");
#endif
    if(p_sectionCourante !=NULL) {
        switch (st_currentSection) {
            case RUNNING_SECTION:
                break;
            case LOADING_SECTION:
                p_sectionCourante=p_sectionCourante->nextSection;
                tachySectionDist_cm = 0;
                break;
            case ARRET:
                //on est à l'arret
                break;
            default:
                break;
        }
    }
    return;
}


void maxSpeedOutput(void)
{
#if (DEBUG > 3)
    pc.printf("\r\n Output MAX SPEED\r\n");
#endif
    if(p_sectionCourante !=NULL) {
        switch(st_maxSpeed) {
            case BLOCKED:
                maxSpeed_cmps = 0;
                break;
            case SPEED_WARNING:
                maxSpeed_cmps = p_sectionCourante->slowSpeed_cmps;
                break;
            default:
                maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps;
                break;
        }
    } else {
        maxSpeed_cmps = 0.0;
    }
    return;
}

void throttleOutput(void)
{
#if (DEBUG > 3)
    pc.printf("\r\n Output TROTTLE\r\n");
#endif
    switch (st_thro) {
        case REGULATION_SPEED:
            pulseSpeed_us = maxSpeed_cmps * 279 / 2048 + 1558 ;
            break;
        case BRAKING:
#if DEBUG > 2
            pc.printf("BRAKINGGGGGGGGGGGGGGGGGG !!! \r\n");
#endif
            pulseSpeed_us = BRAKING_PULSE_US;
            break;
        case STOPPED:
#if DEBUG > 2
            pc.printf("STOPPED\r\n");
#endif
            pulseSpeed_us = ZERO_PULSE_SPEED_US;
            break;
        default:
            break;
    }

    PwmMotor.pulsewidth_us(pulseSpeed_us);
#if DEBUG > 1
    pc.printf("PWM Thro pulse: %.4lf micros\r\n",pulseSpeed_us);
#endif
    return;
}