File content as of revision 7:64bfecbb3000:

#include "mbed.h"
#include "fct.h"
#include "CMPS03.h"
void contournement()
{
    sensMG.write(0);
    sensMD.write(0);
    cmdD=35;
    cmdG=15;
    MD.pulsewidth(vitesse(cmdD));
    MG.pulsewidth(vitesse(cmdG));

}
void rotation_horaire()
{
    sensMG.write(0);
    sensMD.write(1);
    cmdD=30;
    cmdG=30;
    MD.pulsewidth(vitesse(cmdD));
    MG.pulsewidth(vitesse(cmdG));
}
void stop()
{
    sensMG.write(0);
    sensMD.write(0);
    cmdD=0;
    cmdG=0;
    MD.pulsewidth(vitesse(cmdD));
    MG.pulsewidth(vitesse(cmdG));
}
void suivi_mur()
{
    E1=E0;
    E0=US2-US1;
    if((E0+E1)>0) {
        cmdD=VMAX;
        cmdG=VMAX-Kp_ecart*(E0+E1)-Kp_dist*(US2-45);
    } else {
        cmdD=VMAX+Kp_ecart*(E0+E1)+Kp_dist*(US2-45);
        cmdG=VMAX;
    }

    MD.pulsewidth(vitesse(cmdD));
    MG.pulsewidth(vitesse(cmdG));

    wait(0.001);

}
void init()
{
    sensMG.write(0);
    sensMD.write(0);
    MG.period(PERIOD);
    MD.period(PERIOD);
    MG.pulsewidth(vitesse(0));
    MD.pulsewidth(vitesse(0));
    tic1.attach(&fcttrig,0.035);
    tic2.attach(&mesAN,0.01);
    echo.rise(&start_trig);
    echo.fall(&stop_trig);
    bearing_set=boussole.readBearing()/10.0;
}
void mesAN()
{
    if(flag4==0) {
        AN1_av=AN1;
    }
    AN1=0.82*a/(3.3*AnaG.read()-b);
    if(((AN1-AN1_av)>40)||((AN1-AN1_av)<-40)) {
        float temp=AN1;
        AN1=AN1_av;
        AN1_av=temp;
        flag4=1;
    } else {
        flag4=0;
    }
    if(AN1<0||AN1>150)AN1=150;


    if(flag5==0) {
        AN2_av=AN2;
    }
    AN2=0.82*a/(3.3*AnaAV.read()-b);

    if(((AN2-AN2_av)>40)||((AN2-AN2_av)<-40)) {
        float temp=AN2;
        AN2=AN2_av;
        AN2_av=temp;
        flag5=1;
    } else {
        flag5=0;
    }
    if(AN2<0||AN2>150)AN2=150;
}

void fcttrig()
{
    switch(drap) {
        case 1 :
            trigger2.write(1);
            wait_us(10);
            trigger2.write(0);
            drap=2;
            break;
        case 2 :
            trigger3.write(1);
            wait_us(10);
            trigger3.write(0);
            drap=3;
            break;
        case 3 :
            trigger1.write(1);
            wait_us(10);
            trigger1.write(0);
            drap=1;
            break;
    }

}
void start_trig()
{
    temp.reset();
    temp.start();
}
void stop_trig()
{
    temp.stop();
    switch(drap) {
        case 1 :
            if(flag3==0) {
                US3_av=US3;
            }
            US3=temp.read_us()/58.31;
            if(((US3-US3_av)>50)||((US3-US3_av)<-50)) {
                float temp=US3;
                US3=US3_av;
                US3_av=temp;
                flag3=1;
            } else {
                flag3=0;
            }
            if(US3<0||US3>150)US3=150;
            break;
        case 2 :
            if(flag2==0) {
                US2_av=US2;
            }
            US2=temp.read_us()/58.31;
            if(((US2-US2_av)>50)||((US2-US2_av)<-50)) {
                float temp=US2;
                US2=US2_av;
                US2_av=temp;
                flag2=1;
            } else {
                flag2=0;
            }
            if(US2<0||US2>150)US2=150;
            break;
        case 3 :
            if(flag1==0) {
                US1_av=US1;
            }
            US1=temp.read_us()/58.31;
            if(((US1-US1_av)>50)||((US1-US1_av)<-50)) {
                float temp=US1;
                US1=US1_av;
                US1_av=temp;
                flag1=1;
            } else {
                flag1=0;
            }
            if(US1<0||US1>150)US1=150;
            break;
    }
}
float vitesse(float vit)
{
    if(vit<0) vit=0;
    if(vit>VLIMIT) vit=VLIMIT;
    return ((vit/100.0)*PERIOD);
}