File content as of revision 25:869b1c1f51a7:

/* #include */
#include "pins.h"

/* #define */
#define DISTLIM 600 // Distance max de détection
#define OFFSET 200
#define MAXMOY 3 // Nombre de mesure pour la moyenne

/* Variables globales */
Timer TimUS; // Timer pour la mesure de distance entre le robot
Ticker TickUS_actu; // Actualisation valeur distance détection d'obstacle

unsigned int us_high[6] = {0}; // Valeurs du timer lorsqu'il y a une interruption sur front montant
unsigned int us_low[6] = {0}; // Valeurs du timer lorsqu'il y a une interruption sur front descendant
unsigned int us_diff[6] = {0}; // Différence entre ces deux temps
//bool us_verif[6] = {0}; // Permet d'enchainer dans le bon ordre : front montant puis front descendant
bool us_verif2[2][6] = {0};

unsigned int us_dist[6] = {0}; // Valeurs des distances
bool us_rbt_restart = 0; // Le robot a détecté un obstacle et s'arrête
bool us_libre = 0; // Le robot ne détecte plus d'obstacle, il est prêt à redémarrer

int i = 0;
unsigned int us_dist_total[6] = {0};
unsigned int us_dist_moy[6] = {0};

void captUS_moyenne()
{
    if(i < MAXMOY) {
        for(int j=0; j<6 ; j++) {
            us_dist_total[j] += us_dist[j];
        }
        i++;

    } else {
        for(int j=0; j<6 ; j++) {
            us_dist_moy[j]=us_dist_total[j] / MAXMOY;
            us_dist_total[j]=0;
        }

        i=0;
    }
}


void captUS_trig()
{
    captUS_convToDist();

    captUS_moyenne();

    if((objRecule[indiceStrategie]==0) && (action == 2)) {
        if ((us_dist_moy[5] >= (DISTLIM-OFFSET)) && (us_dist_moy[0] >= DISTLIM)  && (us_dist_moy[1] >= (DISTLIM-OFFSET))) {
            us_libre = 1;
        }

        else {
            mot_dis();
            us_rbt_restart = 1;
            us_libre=0;
        }
    }

    else if((objRecule[indiceStrategie]==1) && (action == 2)) {
        if ((us_dist_moy[2] >= (DISTLIM-OFFSET)) && (us_dist_moy[3] >= DISTLIM)  && (us_dist_moy[4] >= (DISTLIM-OFFSET))) {
            us_libre=1;
        }

        else {
            mot_dis();
            us_rbt_restart = 1;
            us_libre=0;
        }
    }

    TimUS.reset();
    trigger=1;
    wait(0.00002);
    trigger=0;
}

void echoRise1()
{
    if(us_verif2[1][0] == false) {
        us_high[0]=TimUS.read_us();
        us_verif2[0][0] = true;
    }
}

void echoFall1()
{
    if(us_verif2[0][0] == true) {
        us_low[0]=TimUS.read_us();
        us_verif2[1][0] = true;
    }
}

void echoRise2()
{
    if(us_verif2[1][1] == false) {
        us_high[1]=TimUS.read_us();
        us_verif2[0][1] = true;
    }
}

void echoFall2()
{
    if(us_verif2[0][1] == true) {
        us_low[1]=TimUS.read_us();
        us_verif2[1][1] = true;
    }
}

void echoRise3()
{
    if(us_verif2[1][2] == false) {
        us_high[2]=TimUS.read_us();
        us_verif2[0][2] = true;
    }
}

void echoFall3()
{
    if(us_verif2[0][2] == true) {
        us_low[2]=TimUS.read_us();
        us_verif2[1][2] = true;
    }
}

void echoRise4()
{
    if(us_verif2[1][3] == false) {
        us_high[3]=TimUS.read_us();
        us_verif2[0][3] = true;
    }
}

void echoFall4()
{
    if(us_verif2[0][3] == true) {
        us_low[3]=TimUS.read_us();
        us_verif2[1][3] = true;
    }
}

void echoRise5()
{
    if(us_verif2[1][4] == false) {
        us_high[4]=TimUS.read_us();
        us_verif2[0][4] = true;
    }
}

void echoFall5()
{
    if(us_verif2[0][4] == true) {
        us_low[4]=TimUS.read_us();
        us_verif2[1][4] = true;
    }
}

void echoRise6()
{
    if(us_verif2[1][5] == false) {
        us_high[5]=TimUS.read_us();
        us_verif2[0][5] = true;
    }
}

void echoFall6()
{
    if(us_verif2[0][5] == true) {
        us_low[5]=TimUS.read_us();
        us_verif2[1][5] = true;
    }
}


void captUS_convToDist()
{
    /**************************************
     * Nous convertisons grâce au valeur  *
     * qui sont retournées par echoRiseX  *
     * et echoFallx                       *
     **************************************/

    for(int i = 0; i<6; i++) {
        if(us_verif2[0][i] == true && us_verif2[1][i] == true)
            us_dist[i] = (int) (((us_low[i] - us_high[i])*340)/1000); //conversion en distance(mm)
        else
            us_dist[i] = LARGEUR_TAB;
    }
}