File content as of revision 2:2e759e231fb6:

#include "mbed.h"
#include "Servo.h"

#define velocidade(a, b)    MDPWM = a; MEPWM = b 
#define frente()            MDdirect = 1; MEdirect = 1
#define re()                MDdirect = 0; MEdirect = 0
#define esquerda()          MDdirect = 1; MEdirect = 0
#define direita()           MDdirect = 0; MEdirect = 1

//Sonar Frente
//DigitalOut trig(PTB2,0);      //Configuração do pino de Trigger
//InterruptIn echo(D12);       //Configuração da interrupção por pino de Echo 




DigitalOut bip(D2);         //
Servo myservo(D3);          //
DigitalOut MEdirect(D4);    //Motor 2 Direction control
PwmOut MEPWM(D5);           //Motor 2 PWM control
PwmOut MDPWM(D6);           //Motor 1 PWM control
DigitalOut MDdirect(D7);    //Motor 1 Direction control


DigitalOut trigF(A2,0);     //Configuração do pino de Trigger
DigitalIn echoF(D12);     //Configuração da interrupção por pino de Echo  

//Sonar Direito
DigitalOut trigD(A1, 0);     //Configuração do pino de Trigger
DigitalIn echoD(D10);     //Configuração da interrupção por pino de Echo 

//Sonar Esqerdo
DigitalOut trigE(A0,0);      //Configuração do pino de Trigger
InterruptIn echoE(D11);      //Configuração da interrupção por pino de Echo 

DigitalOut Led(LED1);

Serial pc(USBTX,USBRX);     //Configuração da comunicação serial para enviar o valor do sensor
Timer tempo;                //Para usar rotinas de tempo



void iniP(void);     
void finP(void); 
float lerSonarF(void);
float lerSonarD(void);
float lerSonarE(void);
void sonarFrente(void);

float tdist=0, distcm=0, distin=0, dist0=0;
float dist_esq = 0, dist_dir = 0, dist_fre = 0, razDE = 0, Xmeio = 0, Ve = 0.3, Vd = 0.3, Vg = 0.23, Kp = 0.080, ed = 0, ee = 0;
char flagTempo = 0;
  
int main() { 
    //printf("INICIA\n");
    //myservo.calibrate(0.0013, 45.0);
    
            //
    
    wait_ms(3000);
    myservo.position(-3.0);
    Led = 1; 
    
    //printf("COMECA O PROGRAMA\n");
    frente();
    velocidade(Vd, Ve);
    wait_ms(500);
    
    while(1){
        dist_fre = lerSonarF(); 
        if (dist_fre <= 20){
            velocidade(0.0, 0.0);
        }      
        //sonarFrente();
        //printf("Distancia detectada pelo sensor Frente %.2f cm \n",dist_fre);        
        //wait_ms(1000);
        
        dist_esq = lerSonarE();
        //printf("Distancia detectada pelo sensor esquerdo %.2f cm \n",dist_esq);
        //wait_ms(1000);
        
        dist_dir = lerSonarD();
        //printf("Distancia detectada pelo sensor direito %.2f cm \n",dist_dir);
        //wait_ms(1000);
        
        Xmeio = (dist_dir + dist_esq) / 2;  //Caucula o SetPoint
        
        ed = Xmeio - dist_dir;              //
        ee = Xmeio - dist_esq;              //
        
        Ve = Vg + (ee * Kp);                //
        Vd = Vg + (ed * Kp);                //
        
        //Ve =(ee * Kp);                //
        //Vd =(ed * Kp);
        
        if(Ve > 0.38){
            Ve = 0.38;
        }
        if(Vd > 0.38){
            Vd = 0.38;
        }
        if(Ve < 0.18){
            Ve = 0.18;
        }
        if(Vd < 0.18){
            Vd = 0.18;
        }
        velocidade(Vd, Ve);
    }
}

float lerSonarD(void)
{
    trigD = 1;                  //Inicio do trigger
    wait_us(10);                //10us de pulso
    trigD = 0;                  //Fim do trigger
    while(!echoD);
    tempo.start();
    while(echoD);
    tdist = tempo.read_us();    //Leitura do tempo transcorrido
    distcm = tdist/58;          //Cálculo da distância detectada em "cm"
    //distin = tdist/148;       //Cálculo da distância detectada em "in"  
    tempo.stop();               //Paro o temporizador
    tempo.reset();              //Reset para o próximo ciclo
    return distcm;
}

float lerSonarF(void)
{
    trigF=1;                    //Inicio do trigger
    wait_us(10);                //10us de pulso
    trigF=0;                    //Fim do trigger
    while(!echoF);
    tempo.start();
    while(echoF);
    tdist = tempo.read_us();    //Leitura do tempo transcorrido
    distcm = tdist/58;          //Cálculo da distância detectada em "cm"
    //distin = tdist/148;       //Cálculo da distância detectada em "in"  
    tempo.stop();               //Paro o temporizador
    tempo.reset();              //Reset para o próximo ciclo
    return distcm;
}

float lerSonarE(void)
{
    trigE = 1;                  //Inicio do trigger
    wait_us(10);                //10us de pulso
    trigE = 0;                  //Fim do trigger
    while(!echoE);
    tempo.start();
    while(echoE);
    tdist = tempo.read_us();    //Leitura do tempo transcorrido
    distcm = tdist/58;          //Cálculo da distância detectada em "cm"
    //distin = tdist/148;       //Cálculo da distância detectada em "in"  
    tempo.stop();               //Paro o temporizador
    tempo.reset();              //Reset para o próximo ciclo
    return distcm;
}


void sonarFrente(void){
    bip = 1;
    printf("PARA\n");
    re();
    wait_ms(100);
    velocidade(0.0, 0.0);
    bip = 0;
            
    myservo.position(-33.0);
    wait_ms(1000);
    lerSonarF();
    printf("Distancia direita %.2f cm \n",distcm);
    dist_dir = distcm;
            
    myservo.position(30.8);
    wait_ms(1000);
    lerSonarF();
    printf("Distancia esquerda %.2f cm \n",distcm);
    dist_esq = distcm;
            
    if(dist_dir > dist_esq){
        printf("VIRA ESQUERDA \n");
        direita();
        velocidade(0.48, 0.48);
        wait_ms(250);
        velocidade(0.0, 0.0);
        myservo.position(-3.0);
        wait_ms(500);
        velocidade(0.48, 0.48);
        frente();
    }
    else if(dist_dir < dist_esq){
        printf("VIRA ESQUERDA \n");
        esquerda();
        velocidade(0.48, 0.48);
        wait_ms(250);
        velocidade(0.0, 0.0);
        myservo.position(-3.0);
        wait_ms(500);
        velocidade(0.48, 0.48);
        frente();   
        }
}