File content as of revision 0:dcb865a03d57:

#include "mbed.h"
#define PERIOD 0.0001
#define VMOY 25
#define VMAX 60
#define Kp_E 0.0
#define Kp_ecart 0.8
#define Td_ecart 0.0
#define Ti_ecart 1000.0
#define limitmin 3
#define limitmax 150
#define Te 0.001
#define Ti 2.0
#define a 24.0
#define b 0.1


BusOut leds(LED1,LED2,LED3,LED4);
DigitalOut trigger1(p14);
DigitalOut trigger2(p16);
DigitalOut trigger3(p18);
InterruptIn echo(p11);
AnalogIn AnaG(p17);
AnalogIn AnaAV(p15);
PwmOut MG(p21); //vitesse moteur gauche
PwmOut MD(p24); //vitesse moteur droit
DigitalOut sensMG(p23);  // sens moteur gauche
DigitalOut sensMD(p26);  // sens moteur droit
Timer temp,t;
Ticker tic1,tic2;

//GLOBALES
int drap=1;
float US1,US2,US3,AN1,AN2,US1_av=50,US2_av=50,US3_av=50,AN1_av=50,AN2_av=50;
float E_av,E,iE=0;
float cmdG=0,cmdD=0;
int etat=0;
float iecart=0,ecart_av,ecart;

//PROTOTYPES
void asservissement();
void fcttrig();
float vitesse(float);
void start();
void stop();
float vitesse(float);
void mesAN();

int main()
{

    sensMG.write(1);
    sensMD.write(1);
    MG.period(PERIOD);
    MD.period(PERIOD);
    MG.pulsewidth(vitesse(0));
    MD.pulsewidth(vitesse(0));
    tic1.attach(&fcttrig,0.05);
    tic2.attach(&mesAN,0.01);
    echo.rise(&start);
    echo.fall(&stop);
    while(1) {
        printf("US1=%.0f US2=%.0f US3=%.0f -- AN1=%.0f AN2=%.0f \n\r",US1,US2,US3,AN1,AN2);
        wait(0.1);
    }

}
void mesAN()
{
    AN1_av=AN1;
    AN2_av=AN2;
    AN1=a/(3.3*AnaG.read()-b);
    AN2=a/(3.3*AnaAV.read()-b);
  /*  if(((AN1-AN1_av)>50)||((AN1-AN1_av)<-50)) {
        AN1=AN1_av;
    }
    if(((AN2-AN2_av)>50)||((AN2-AN2_av)<-50)) {
        AN2=AN2_av;
    }
    */
    if(AN1<0||AN1>150)AN1=150;
    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()
{
    temp.reset();
    temp.start();
}
void stop()
{
    temp.stop();
    switch(drap) {
        case 1 :
            US3_av=US3;
            US3=temp.read_us()/58.31;
         //   if(((US3-US3_av)>50)||((US3-US3_av)<-50)) {
         //       US3=US3_av;
         //   }
            break;
        case 2 :
            US2_av=US2;
            US2=temp.read_us()/58.31;
         //   if(((US2-US2_av)>50)||((US2-US2_av)<-50)) {
         //       US2=US2_av;
         //   }
            break;
        case 3 :
            US1_av=US1;
            US1=temp.read_us()/58.31;
          //  if(((US1-US1_av)>50)||((US1-US1_av)<-50)) {
          //      US1=US1_av;
          //  }
            break;
    }
}
float vitesse(float vit)
{
    if(vit<0) vit=0;
    if(vit>VMAX) vit=VMAX;
    return ((vit/100.0)*PERIOD);
}