File content as of revision 3:e5d8eb3e3951:

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

#define velocidade(a, b)    MDPWM = a; MEPWM = b 
#define sentido(a, b)       MDdirect = a; MEdirect = b


//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(PTB0);          //

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

HCSR04 sonarD(D9, D8);
HCSR04 sonarE(D11, D10);
HCSR04 sonarF(D13, D12);

DigitalOut Led(LED2);

Serial pc(USBTX,USBRX);     //Configuração da comunicação serial para enviar o valor do sensor

void sonarFrente(void);

float dist_esq = 0, dist_dir = 0, dist_fre = 0, razDE = 0, Xmeio = 0, Ve = 0.3, Vd = 0.3, Vg = 0.0, Kp = 0.050, ed = 0, ee = 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 = sonarF.getCm(); 
        /*if (dist_fre <= 20){
            velocidade(0.0, 0.0);
        }*/      
        //printf("Distancia detectada pelo sensor Frente %.2f cm \n",dist_fre);        
        //wait_ms(1000);
        
        dist_esq = sonarE.getCm();
        //printf("Distancia detectada pelo sensor esquerdo %.2f cm \n",dist_esq);
        //wait_ms(1000);
        
        dist_dir = sonarD.getCm();
        //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.31){
            Ve = 0.31;
        }
        if(Vd > 0.31){
            Vd = 0.31;
        }
        if(Ve < 0.18){
            Ve = 0.18;
        }
        if(Vd < 0.18){
            Vd = 0.18;
        }
        velocidade(Vd, Ve);
    }
}



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