File content as of revision 0:182a12372dd4:

#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 
//Sonar Esquerdo
//DigitalOut trig(A0,0);      //Configuração do pino de Trigger
//InterruptIn echo(D11);       //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
//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 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 lerSonar(void);

float tdist=0, distcm=0, distin=0, dist0=0;
float dist_esq = 0, dist_dir = 0, dist_fre = 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.38, 0.38);
    frente();
    
    while(1){
        lerSonar();
        printf("Distancia detectada pelo sensor %.2f cm \n",distcm);
        dist_fre = distcm;
    
        if((dist_fre <= 30) && (dist_fre > 3)){
            bip = 1;
            printf("PARA\n");
            re();
            wait_ms(100);
            velocidade(0.0, 0.0);
            bip = 0;
            
            myservo.position(-33.0);
            wait_ms(1000);
            lerSonar();
            printf("Distancia direita %.2f cm \n",distcm);
            dist_dir = distcm;
            
            myservo.position(30.8);
            wait_ms(1000);
            lerSonar();
            printf("Distancia esquerda %.2f cm \n",distcm);
            dist_esq = distcm;
            
            if(dist_dir > dist_esq){
                printf("VIRA ESQUERDA \n");
                direita();
                velocidade(0.38, 0.38);
                wait_ms(250);
                velocidade(0.0, 0.0);
                myservo.position(-3.0);
                wait_ms(500);
                velocidade(0.38, 0.38);
                frente();
            }
            else if(dist_dir < dist_esq){
                printf("VIRA ESQUERDA \n");
                esquerda();
                velocidade(0.38, 0.38);
                wait_ms(250);
                velocidade(0.0, 0.0);
                myservo.position(-3.0);
                wait_ms(500);
                velocidade(0.38, 0.38);
                frente();   
            }
            //myservo.position(-3.0);
            //wait_ms(100);
        }
    }
    
    
    
    /*while(1){
        M1direct = 1;
        M2direct = 1;
        bip = 0;
        
        for(float p=0; p<=1.0; p += 0.1){
            myservo = p;
            trig=1;             //Inicio do trigger
            wait_us(10);        // 10us de pulso
            trig=0;             //Fim do trigger
            echo.rise(&iniP);   //leitura do inicio do pulso de retorno
            echo.fall(&finP);   //Leitura do final do pulso de retorno
            printf("Distancia detectada pelo sensor %.2f cm \n",distcm);
            wait(0.05);
        }
        wait(0.13);
        
        M1direct = 1;
        M2direct = 1;
        bip = 0;
        
        for(float p=1.0; p>=0.0; p -= 0.1) {
            myservo = p;
            trig=1;             //Inicio do trigger
            wait_us(10);        // 10us de pulso
            trig=0;             //Fim do trigger
            echo.rise(&iniP);   //leitura do inicio do pulso de retorno
            echo.fall(&finP);   //Leitura do final do pulso de retorno
            printf("Distancia detectada pelo sensor %.2f cm \n",distcm);
            wait(0.05);
        }
        wait(0.13);
    }*/
}

//Rotina para receber o pulso inicial do pino Echo
void iniP(void)
{            
    tempo.start();              //Rotina para iniciar o contador
    return;
}
//Rotina para pegar o tempo final do pulso   
void finP(void)
{ 
    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;
}

void lerSonar(void)
{
    trig=1;             //Inicio do trigger
    wait_us(10);        //10us de pulso
    trig=0;             //Fim do trigger
    echo.rise(&iniP);   //leitura do inicio do pulso de retorno
    echo.fall(&finP);   //Leitura do final do pulso de retorno
    wait_ms(100);
    return;
}