File content as of revision 20:017ec2428e06:

#include "stateMachines.h"
#if DEBUG >0
Serial pc(USBTX, USBRX);    // tx, rx
#endif
#if DEBUG >= -1
InterruptIn my_button(USER_BUTTON);
#endif

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
uint16_t distMurG90Moy;
uint16_t distMurD90Moy;
uint16_t distMurG45Moy;
uint16_t distMurD45Moy;
#ifdef DLVV
uint16_t distMurG10Moy;
uint16_t distMurD10Moy;
uint16_t distMurFrontMoy
#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;
//sections
s_Section p_section1;

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

int pulseDirection = DIRECTION_PULSE_MIDDLE;
int pulseSpeed_us = INITAL_PULSE_SPEED_US;
//Capteurs direction

AnalogIn anaG90(PC_1);//capteur ir coté gauche
AnalogIn anaD90(PA_1);//capteur ir coté droit
AnalogIn anaG45(PC_0);//capteur ir avant gauche 45 deg
AnalogIn anaD45(PC_2);//capteur ir avant droit 45deg
#ifdef DLVV
AnalogIn anaDlvvG(PB_0);//capteur ir avant droit 10 deg
AnalogIn anaDlvvD(PC_3);//capteur ir coté droit 10 deg
AnalogIn anaDlvvFront(PA_4);//capteur ir avant
#endif

int differenceGD45,differenceGD90,prevDiffGD90,prevDiffGD45,derive45,derive90;

//Capteur vitesse
InterruptIn it_tachymeter(PA_11);


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

int distLidar;// LiDAR actually measured distance value
int strengthLidar;// LiDAR signal strength
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
int maxSpeed_cmps = 0;
int tachySpeed_cmps = 0;
int tachyStepsRegister = 0;
int tachySectionDist_cm = 0;

//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;


// +++++++++++++++++++++++++++++++++++++++++++ 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].cg45 = 0;
        history[m].cd45 = 0;
        history[m].cg90 = 0;
        history[m].cd90 = 0;
        history[m].lidarDist = 0;
        history[m].lidarStr = 0;
        history[m].speed = 0;
    }
    #if DEBUG > 0
    pc.printf("[INIT SAMPLE DONE]\r\n");
    #endif
    return;
}
void sampleLog(void)
{
    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].cg45 = distMurG45Moy;
        history[indexSample].cd45 = distMurD45Moy;
        history[indexSample].cg90 = distMurG90Moy;
        history[indexSample].cd90 = distMurD90Moy;
        history[indexSample].lidarDist = distLidar;
        history[indexSample].lidarStr = strengthLidar;
        history[indexSample].speed = tachySpeed_cmps;
        history[indexSample].dist = tachySectionDist_cm,
        indexSample++;
#if DEBUG > 0
        pc.printf("\r\nodo:%d  dist = %d    \tstrength = %d    \tC45D: %d C45G: %d  C90D: %d  C90G: %d  looptime: %d micros",tachySectionDist_cm,distLidar,strengthLidar,distMurD45Moy,distMurG45Moy,distMurD90Moy,distMurG90Moy,timeSinceStart.read_us());// output signal strength value
        pc.printf("\r\nstate Murs: %d, state Section %d, state MaxSpeed %d, state Throttle %d\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro);
        //wait(2);
        timeSinceStart.reset();
        timeSinceStart.start();
#endif
    }
    return;
}
void transmitData(void)
{
    #if DEBUG > 0
    pc.printf("[START TO TRANSMIT]\r\n");
    #endif
    serialLidar.printf("time,gauche 45,droite 45,gauche 90, droite 90,dist,lidar dist,strength,speed,murs/dlvv,section,maxSpeed,throttle\r\n");
    for(int p=0; p<indexSample; p++) {
        serialLidar.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",
        history[p].time,
        history[p].cg45,
        history[p].cd45,
        history[p].cg90,
        history[p].cd90,
        history[p].dist,
        history[p].lidarDist,
        history[p].lidarStr,
        history[p].speed,
        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
    //p_sectionCourante = &p_section1;
    transmitData();
}
#endif

void getTachySpeed()
{
    //tachySteps = VALEUR_DU_PIN;
    //poour gérer les vitesses lentes
    if(tachyStepsRegister == 0 && timerSinceGetTachy.read_us() < 500000) {
        return;//on attend encore un peu l'aquisition de la vitesse
    }
    tachySectionDist_cm += tachyStepsRegister;
    tachySpeed_cmps = (tachyStepsRegister * 1000000)/timerSinceGetTachy.read_us();
#if DEBUG > 2
    pc.printf("IT: distance parcourue %d       , vitesse:%d         \r\n",tachySectionDist_cm,tachySpeed_cmps);
#endif
    tachyStepsRegister=0;
    timerSinceGetTachy.reset();
    timerSinceGetTachy.start();
    return;
}

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

void it4cm()
{
    tachyStepsRegister+=4;
#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
                distLidar=uart[2]+uart[3]*256;// calculate distance value
                strengthLidar=uart[4]+uart[5]*256;// calculate signal strength value
            }
        }
    }
}
// +++++++++++++++++++++++++++++++++++++++++++ STATES MACHINES +++++++++++++++++++++++++++++++++++++++++++

//########## INIT STATES MACHINES ##########
void mursInit(void)
{
#if DEBUG >= -1
    my_button.fall(&pressed);
    initSamples();
#endif
#if DEBUG > 0
    pc.baud(115200);
    pc.printf("Init Murs\r\n");
#endif
    timeSinceStart.start();
    st_murs=EQUILIBRAGE_REPULSIF;
    PwmDirection.period_us(SPEED_PERIOD_US);
    PwmDirection.pulsewidth_us(DIRECTION_PULSE_MIDDLE);// milieu
    prevDiffGD45 = 0;
    prevDiffGD90 = 0;
    return;
}
#ifdef DLVV
void obstacleInit(void)
{
#if DEBUG > 0
    pc.printf("Init Obstacle\r\n");
#endif
    st_speedLimit=FRONT_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);
    timerSinceGetTachy.start();
    getTachySpeed();//to reset
    tachySectionDist_cm = 0;
    tachyStepsRegister = 0;

    //section de test
    p_section1.nextSection = NULL;
    p_section1.targetSpeed_cmps = 300;
    p_section1.slowSpeed_cmps = 5;
    p_section1.brakingCoefficient = 570; // application de la formule
    p_section1.coef_p_accel = 1;
    p_section1.lidarWarningDist_cm = 300;
    p_section1.lng_section_cm = 1600;//1600cm
    p_section1.coef_p = 35;
    p_section1.coef_i = 0;
    p_section1.coef_d = 20;
    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 = UNDER_SPEED;
    PwmMotor.period_us(DIERCTION_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: %d micros\r\n",timeSinceStart.read_us());
    pc.printf("\r\nStates INIT: state Murs: %d, state Section %d, state MaxSpeed %d, state Throttle %d\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro);
    timeSinceStart.reset();
    timeSinceStart.start();
#endif
    return;
}


//########## UPDATE STATES ##########
void mursUpdate(void)
{
#if (DEBUG > 3)
    pc.printf("\r\nUpdate MURS\r\n");
#endif
    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;

    distMurG45Moy = getDistMoy(distMurG45,NB_ECHANTILLONS_IR);
    distMurD45Moy = getDistMoy(distMurD45,NB_ECHANTILLONS_IR);

#if DEBUG > 1
    pc.printf("dist45G:%d       dist45D:%d        Deadzone: %d\r\n",distMurG45Moy,distMurD45Moy,IR_DEADZONE_U16_22cm);
#endif

    switch (st_murs) {
        case EQUILIBRAGE_REPULSIF:
            distMurG90Moy = getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
            distMurD90Moy = getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
            if( distMurG90Moy <= IR_DEADZONE_U16_22cm) {
                st_tmpMurs = ATTRACTIF_D;
            } else if(distMurD90Moy  <= IR_DEADZONE_U16_22cm) {
                st_tmpMurs = ATTRACTIF_G;
            } else {
                st_tmpMurs = EQUILIBRAGE_REPULSIF;
            }
            break;
        case ATTRACTIF_D:
            distMurD90Moy= getDistMoy(distMurD90,NB_ECHANTILLONS_IR);
            if( distMurD90Moy >= IR_FAR_U16_105cm) {
                st_tmpMurs = ATTRACTIF_D;
            } else {
                st_tmpMurs = EQUILIBRAGE_REPULSIF;
            }
            break;
        case ATTRACTIF_G:
            distMurG90Moy= getDistMoy(distMurG90,NB_ECHANTILLONS_IR);
            if(distMurG90Moy >= IR_FAR_U16_105cm) {
                st_tmpMurs = ATTRACTIF_G;
            } else {
                st_tmpMurs = EQUILIBRAGE_REPULSIF;
            }
            break;
        default:
            return;
    }
    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
    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;
            } 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
    MAX_SPEED_ST st_tmpMaxSpeed;
    i=0;

    if( strengthLidar > (LIDAR_STRENGTH_THRESOLD + LIDAR_STRENGH_DELTA) ) {
        if(distLidar > 30 && distLidar < p_sectionCourante->lidarWarningDist_cm) {
            st_tmpMaxSpeed = SPEED_VARIABLE;
        } else if( distLidar > p_sectionCourante->lidarWarningDist_cm ) {
            st_tmpMaxSpeed = SPEED_MAX;
        } 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)
{
#if (DEBUG > 3)
    pc.printf("\r\nUpdate Throttle\r\n");
#endif
    THROTTLE_ST st_tmpThro;
    getTachySpeed();
    switch (st_thro) {
        case UNDER_SPEED:
            if(st_currentSection == ARRET) {
                st_tmpThro = BRAKING;
            } else if( tachySpeed_cmps >= maxSpeed_cmps ) {
                st_tmpThro = AT_SPEED;
            } else {
                return;
            }
            break;
        case OVER_SPEED:
            if(st_currentSection == ARRET) {
                st_tmpThro = BRAKING;
            } else if( tachySpeed_cmps <= maxSpeed_cmps) {
                st_tmpThro = AT_SPEED;
            } else {
                return;
            }
            break;
        case AT_SPEED:
            if(st_currentSection == ARRET) {
                st_tmpThro = BRAKING;
            } else 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;
        case BRAKING:
            st_tmpThro = STOPPED;
            break;
        case STOPPED:
            if(st_currentSection == RUNNING_SECTION) {
                if( tachySpeed_cmps < maxSpeed_cmps ) {
                    st_tmpThro = UNDER_SPEED;
                } else if( tachySpeed_cmps > maxSpeed_cmps ) {
                    st_tmpThro = OVER_SPEED;
                } else {
                    st_tmpThro = AT_SPEED;
                }
            } else {
                st_tmpThro = STOPPED;
            }
            break;
        default:
            break;
    }
    st_thro = st_tmpThro;
    return;
}

//########## OUTPUT STATES ##########
//updating output parameters
void mursOutput(void)
{
#if (DEBUG > 3)
    pc.printf("\r\n Output MURS\r\n");
#endif
    switch (st_murs) {
        case EQUILIBRAGE_REPULSIF://TO DO TODO
        case ATTRACTIF_G:
        case ATTRACTIF_D:

            prevDiffGD90 = differenceGD90;
            prevDiffGD45 = differenceGD45;
            differenceGD90 = distMurD90Moy - distMurG90Moy;
            differenceGD45 = distMurD45Moy - distMurG45Moy;

            derive45 = differenceGD45 - prevDiffGD45;
            derive90 = differenceGD90 - prevDiffGD90;

            pulseDirection = ( ((differenceGD90*2+differenceGD45*4)/8) / p_sectionCourante->coef_p)
                             + (  ((derive90*2+derive90*4)/8) /  p_sectionCourante->coef_d)
                             + 1500;
            if(pulseDirection > DIRECTION_PULSE_MAX) { //POUR TOURNER A GAUCHE
#if DEBUG > 1
                pc.printf("!!! OVER PWM Direction pulse: %d\r\n",pulseDirection);
#endif
                pulseDirection = DIRECTION_PULSE_MAX;
            } else if(pulseDirection < DIRECTION_PULSE_MIN ) { //POUR TOURNER A DROITE
#if DEBUG > 1
                pc.printf("!!! UNDER PWM Direction pulse: %d\r\n",pulseDirection);
#endif
                pulseDirection = DIRECTION_PULSE_MIN ;
            }
        default:
            break;
    }
#if DEBUG > 1
    pc.printf("PWM Direction pulse: %d\r\n",pulseDirection);
#endif
    PwmDirection.pulsewidth_us(pulseDirection);
    return;
}

#ifdef DLVV
void obstacleOutput(void)
{
    return;
}
#endif
void sectionOutput(void)
{
#if (DEBUG > 3)
    pc.printf("\r\n Output Section\r\n");
#endif
    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
    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)
{
#if (DEBUG > 3)
    pc.printf("\r\n Output TROTTLE\r\n");
#endif
    switch (st_thro) {
        case UNDER_SPEED:
            if ( (pulseSpeed_us + p_sectionCourante->coef_p_accel) < MAX_PULSE_WIDTH_FOR_TACHY_US) {
                pulseSpeed_us += p_sectionCourante->coef_p_accel;
            }
            break;
        case OVER_SPEED:
            if ( (pulseSpeed_us - p_sectionCourante->coef_p_accel) > INITAL_PULSE_SPEED_US) {
                pulseSpeed_us -= p_sectionCourante->coef_p_accel;
            }
            break;
        case AT_SPEED:
            break;
        case BRAKING:
#if DEBUG > 2
            pc.printf("BRAKINGGGGGGGGGGGGGGGGGG !!! \r\n");
#endif
            wait(0.5);
            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);
#ifdef SAMPLING
    sampleLog();
#endif
#if DEBUG > 1
    pc.printf("PWM Thro pulse: %d micros\r\n",pulseSpeed_us);
#endif
    return;
}