File content as of revision 1:027b210e631d:

#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); 
void lerSonarF(void);
void lerSonarD(void);
void 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;
char flagTempo = 0;
    
int main() { 
    printf("INICIA\n");
    myservo.calibrate(0.0013, 45.0);
    wait_ms(3000);
    //myservo.position(-33.0); todo pra direita
    //myservo.position(30.8); todo pra esquerda
    myservo.position(-3.0);
    Led = 1;
    printf("COMECA O PROGRAMA\n");
    velocidade(0.48, 0.48);
    frente();
    
    while(1){
        lerSonarF();
        dist_fre = distcm;
        printf("Distancia detectada pelo sensor Frente %.2f cm \n",dist_fre);
        //wait_ms(1000);
        
        lerSonarE();
        dist_esq = distcm;
        printf("Distancia detectada pelo sensor esquerdo %.2f cm \n",dist_esq);
        //wait_ms(1000);
        lerSonarD();
        dist_dir = distcm;
        printf("Distancia detectada pelo sensor direito %.2f cm \n",dist_dir);
        //wait_ms(1000);
        
        razDE = dist_dir / dist_esq;
    
        if((dist_fre <= 30) && (dist_fre > 3)){
            sonarFrente();
        }
        
        if(razDE > 1){
            velocidade(0.22, 0.48);
        }
        else if(razDE < 1){
            velocidade(0.48, 0.22);  
        }   
    }
}

void 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
}

void 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
}

void 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
}


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();   
        }
}