File content as of revision 11:bc24b3ba51a9:

#include "states_m.h"

uint16_t distMurG90[NB_ECHANTILLONS_IR];//buffer tournant ir coté gauche pour moyenne
uint16_t distMurD90[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit pour moyenne
uint16_t distMurG45[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 45deg pour moyenne
uint16_t distMurD45[NB_ECHANTILLONS_IR];//buffer tournant ir avant droit 45deg pour moyenne
#ifdef DLVV
uint16_t distMurG10[NB_ECHANTILLONS_IR];//buffer tournant ir avant gauche 10deg pour moyenne
uint16_t distMurD10[NB_ECHANTILLONS_IR];//buffer tournant ir coté droit 10deg pour moyenne
uint16_t distMurFront[NB_ECHANTILLONS_IR];//buffer tournant ir front
#endif
int dist_obstacle_lidar[NB_ECHANTILLONS_LIDAR];//buffer tournant
int index_fifo_ir = 0;//pour géreer le buffer tournant
int index_fifo_lidar = 0;
int tmpDist;
//sections
s_Section p_section1;


//Capteurs

AnalogIn anaG90(A0);//capteur ir coté gauche
AnalogIn anaD90(A1);//capteur ir coté droit
AnalogIn anaG45(A2);//capteur ir avant gauche 45 deg
AnalogIn anaD45(A3);//capteur ir avant droit 45deg
#ifdef DLVV
AnalogIn anaDlvvG(A4);//capteur ir avant droit 10 deg
AnalogIn anaDlvvD(A5);//capteur ir coté droit 10 deg
AnalogIn anaDlvvFront(A6);//capteur ir avant
#endif
Ticker TIMX(TIMX);//compteur de tours
Digital InterruptSW(D0);//interrupt feu DLVV


//LIDAR
Serial serialLidar(A4,A5);  // tx, rx

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

//SPEED
int maxSpeed_cmps = 0;
int tachySpeed_cmps = 0;
int tachyStepsRegister = 0;
int tachySectionDist_cm = 0;
float ms_pwmSpeedPulse = 0.0015;//IDLE

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

s_Section* p_sectionCourante = NULL;

//time monitoring

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

void getTachySpeed()
{
    //tachySteps = VALEUR_DU_PIN;
    tachySectionDist_cm += tachyStepsRegister * 4;
    tachySpeed_cmps = (tachyStepsRegister * 40000)/timerSinceGetTachy.read_us();
    tachyStepsRegister=0;
    timerSinceGetTachy.reset();
    timerSinceGetTachy.start();
    return;
}

int getDistMoy(int* tab,int size)
{
    int sumMoy = 0;
    for(int k=0; k<size;k++)
    {
        sumMoy+=tab[k];
    }
#if (NB_ECHANTILLONS_IR == 4 && NB_ECHANTILLONS_LIDAR == 4)
    return sumMoy >> 2;
#else
    return sumMoy/size;
#endif
}

//########## INIT STATES MACHINES ##########
void mursInit(void)
{
    st_murs=EQUILIBRAGE_REPULSIF;
    return;
}
#ifdef DLVV
void obstacleInit(void)
{
    st_speedLimit=FRONT_CLEAR;
    return;
}
#endif
void sectionInit(void)
{
    timeSinceStart.reset();
    timeSinceStart.start();
    st_currentSection=ARRET;
    p_sectionCourante->nextSection=&p_section1;
    timerSinceGetTachy.start();
    getTachySpeed();//to reset
    tachySectionDist_cm = 0;
    tachyStepsRegister = 0;

    //section de test
    p_section1.nextSection = NULL;
    p_section1.targetSpeed_cmps = 50;
    p_section1.brakingCoefficient =
            p_section1.ms_accel = 0.00005;
    p_section1.ms_decel = 0.0001;
    p_section1.lidarWarningDist_cm = 200;
    p_section1.slowSpeed_cmps = 15;
    p_section1.lng_section_cm = 1000;//10m
    p_section1.coef_p = 1;
    p_section1.coef_i = 0;
    p_section1.coef_d = 0;
    return;
}

void maxSpeedInit(void)
{
    st_maxSpeed=SPEED_MAX;
    maxSpeed_cmps= p_sectionCourante->targetSpeed_cmps;
    serialLidar.baud(115200);
    return;
}

void throttleInit(void)
{
    st_thro = AT_SPEED;
    return;
}


//########## UPDATE STATES ##########
void mursUpdate(void)
{
    MUR_ST st_tmpMurs;
    //lectures
    distMurG90[index_fifo_ir]=anaG90.read_u16();
    distMurD90[index_fifo_ir]=anaD90.read_u16();
    distMurG45[index_fifo_ir]=anaG45.read_u16();
    distMurD45[index_fifo_ir]=anaD45.read_u16();
    index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR;

    switch (st_murs) {
    case EQUILIBRAGE_REPULSIF:
        tmpDist = getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
        if( tmpDist <= IR_DEADZONE_U16_22cm)
        {
            st_tmpMurs = ATTRACTIF_D;
        }else if(tmpDist  <= IR_DEADZONE_U16_22cm)
        {
            st_tmpMurs = ATTRACTIF_G;
        }else
        {
            st_tmpMurs = EQUILIBRAGE_REPULSIF;
        }
        break;
    case ATTRACTIF_D:
        tmpDist= getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
        if( tmpDist >= IR_FAR_U16_105cm)
        {
            st_tmpMurs = ATTRACTIF_D;
        }else
        {
            st_tmpMurs = EQUILIBRAGE_REPULSIF;
        }
        break;
    case ATTRACTIF_G:
        tmpDist= getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
        if(tmpDist >= IR_FAR_U16_105cm)
        {
            st_tmpMurs = ATTRACTIF_G;
        }else
        {
            st_tmpMurs = EQUILIBRAGE_REPULSIF;
        }
        break;
    default:
        return;
        break;
    }
    st_murs = st_tmpMurs;
    return;
}
#ifdef DLVV
void obstacleUpdate(void)
{
    return;
}
#endif

void sectionUpdate(void)
{
    SECTION_ST st_tmpSection;
    switch (st_currentSection) {
    case RUNNING_SECTION:
        if(tachySectionDist_cm > p_sectionCourante->lng_section_cm)//on pourrait rajouter un test lidar
        {
            st_tmpSection = LOADING_SECTION;
        }
        break;
    case LOADING_SECTION:
        if(p_sectionCourante->nextSection != NULL)
        {
            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)
{
    MAX_SPEED_ST st_tmpMaxSpeed;
    i=0;
    if (serialLidar.readable()) { //check whether the serial port has data input
        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
                    distLidar=uart[2]+uart[3]*256;// calculate distance value
                    strengthLidar=uart[4]+uart[5]*256;// calculate signal strength value
                }
            }
        }
    }

    if( strengthLidar > (LIDAR_STRENGTH_THRESOLD + LIDAR_STRENGH_DELTA) )
    {
        if(distLidar > 30 && distLidar < p_sectionCourante->lidarWarningDist_cm)
        {
            st_tmpMaxSpeed = SPEED_VARIABLE;
        }else
        {
            st_tmpMaxSpeed = SPEED_LIMITED;
        }
    }else if(strengthLidar < (LIDAR_STRENGTH_THRESOLD - LIDAR_STRENGH_DELTA))
    {
        st_tmpMaxSpeed = SPEED_MAX;
    }
    else{
        return;//pour ne pas changer sans arret d'etats lorsque strengthLidar est à la limite du seuil
    }


    st_maxSpeed = st_tmpMaxSpeed;
    return;
}

void throttleUpdate(void)
{
    THROTTLE_ST st_tmpThro;

    switch (st_thro) {
    case UNDER_SPEED:
        if( tachySpeed_cmps > maxSpeed_cmps )
        {
            st_tmpThro = AT_SPEED;
        }else
        {
            return;
        }
        break;
    case OVER_SPEED:
        if( tachySpeed_cmps < maxSpeed_cmps )
        {
            st_tmpThro = AT_SPEED;
        }else
        {
            return;
        }
        break;
    case AT_SPEED:
        if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS) )
        {
            st_tmpThro = OVER_SPEED;
        } else if( tachySpeed_cmps < (maxSpeed_cmps - SPEED_DELTA_CMPS) )
        {
            st_tmpThro = UNDER_SPEED;
        }else
        {
            return;
        }
        break;
    default:
        break;
    }

    st_thro = st_tmpThro;
    return;
}

//########## OUTPUT STATES ##########
//updating output parameters
void mursOutput(void)
{   
    switch (st_murs) {
    case EQUILIBRAGE_REPULSIF:
        break;
    case ATTRACTIF_G:
        break;
    case ATTRACTIF_D:
        break;
    default:
        break;
    }
    return;
}

#ifdef DLVV
void obstacleOutput(void)
{
    return;
}
#endif
void sectionOutput(void)
{

    switch (st_currentSection) {
    case RUNNING_SECTION:
        break;
    case LOADING_SECTION:
        p_sectionCourante=p_sectionCourante->nextSection;
        tachySectionDist_cm = 0;
        break;
    case ARRET:
        wait(10);//on est à l'arret
        break;
    default:
        break;
    }
    return;
}


void maxSpeedOutput(void)
{
    switch(st_maxSpeed)
    {
    case SPEED_LIMITED:
        maxSpeed_cmps = p_sectionCourante->slowSpeed_cmps;
        break;
    case SPEED_VARIABLE:
        maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps * (distLidar/(distLidar+p_sectionCourante->brakingCoefficient));
        break;
    default:
        maxSpeed_cmps = p_sectionCourante->targetSpeed_cmps;
        break;
    }
    return;
}

void throttleOutput(void)
{
    switch (st_thro) {
    case UNDER_SPEED:
        ms_pwmSpeedPulse += p_sectionCourante->ms_accel;
        break;
    case OVER_SPEED:
        ms_pwmSpeedPulse -= p_sectionCourante->ms_decel;
        break;
    case AT_SPEED:
    default:
        break;
    }
    return;
}