File content as of revision 0:7e3e917c6fa8:

#include "mbed.h"
#include "hcsr04.h"
#include "motordriver.h"

//Sensores de Distancia 
HCSR04 usensorA(p10,p11);
HCSR04 usensorD(p6,p5);
HCSR04 usensorI(p20,p19);
//Motores
DigitalOut STBY(p15);
PwmOut PWMA(p26);
DigitalOut AIN1(p30);
DigitalOut AIN2(p29);
PwmOut PWMB(p25);
DigitalOut BIN1(p14);
DigitalOut BIN2(p13);
//Buttons
DigitalIn btn1(p16);
InterruptIn btn2(p17);

//Configuración de interrupciones
//p10,p11,p12,p13
InterruptIn INT0(p22);     //Interrupción externa 0  CHA1
InterruptIn INT1(p21);     //Interrupción externa 1  CHB1
InterruptIn INT2(p23);     //Interrupción externa 2  CHA2
InterruptIn INT3(p24);     //Interrupción externa 3  CHB2
int CNT1, CNT2; //Contadores de posición para encoders
bool flagCHA1, flagCHB1, flagCHA2, flagCHB2;

unsigned int distA, distD, distI;

long ti = 0;
void tiempo(double tiempoControlar, double tiempoInicial)
{
    double ti_transcu = 0;
    double t = clock();
    while (tiempoControlar  > ti_transcu)
    {
       ti_transcu = t - tiempoInicial;
       t = clock();
    }
}
void adelante(){             
    STBY = 1;          
    AIN1 = 1;
    AIN2 = 0;
    PWMA.write(0.32f);
    BIN1 = 1;
    BIN2 = 0;
    PWMB.write(0.34f);
}
void giroDerecha(){            
    STBY = 1;        
    AIN1 = 1;
    AIN2 = 0;
    PWMA.write(0.29f);
    BIN1 = 0;
    BIN2 = 1;
    PWMB.write(0.30f);
}
void giroIzquierda(){        
    STBY = 1;        
    AIN1 = 0;
    AIN2 = 1;
    PWMA.write(0.29f);
    BIN1 = 1;
    BIN2 = 0;
    PWMB.write(0.30f);
}
/*
void corrDer(){
    STBY = 1;        
    AIN1 = 1;
    AIN2 = 0;
    PWMA.write(0.1f);
}
void corrIzq(){
    STBY = 1;        
    BIN1 = 1;
    BIN2 = 0;
    PWMB.write(0.05f);
}*/
void adelantet(long tmedir){
      ti=clock();
      adelante();
      tiempo(tmedir,ti);
}  
void giroDerechat(long tmedir){
      ti=clock();
      giroDerecha();
      tiempo(tmedir,ti);
}  
void giroIzquierdat(long tmedir){
      ti=clock();
      giroIzquierda();
      tiempo(tmedir,ti);
}  
void stopt(long tmedir){
      ti=clock();
      STBY = 0;
      tiempo(tmedir,ti);
} 
void verificarSalida()
{
    //if(distA < distD && distA < distI)
    if(distD >= distI)
        giroDerechat(90); // 180 grados
    else
        if(distI > distD)
           giroIzquierdat(90); // 180 grados
}
void btn2_recorrido() {
    STBY = 0;
    stopt(500);
}

int row = 10;
int col = 10;
int pos;
int posicion(int row, int col)
{
    if(row > 5){
        int nrow = 0;
        nrow = (row - 6);
        row = (5 - nrow);
    }
    if(col > 5){
        int ncol = 0;
        ncol = (col - 6);
        col = (5 - ncol);
    }
    
return (9 -(row - 1) - (col - 1));
}

int main(){

btn2.rise(&btn2_recorrido);//interrupcion para detener  

while(1){
    // recorrer laberinto
    if(btn1.read()){  //Detect Switch Pres  
    stopt(400); //  espera de 4 s
    while(pos != 1){

    distA=usensorA.distance(); wait_ms(90);
    distI=usensorI.distance(); wait_ms(90); 
    distD=usensorD.distance(); wait_ms(90); 
    
    pos = posicion(row, col); // buscar meta
    
    if(distA >= 10) {
      if(distD > 10)
      {
        adelante();  
        giroDerechat(40); //90 grados
       }
        
      /*else
          if(distD == distI){ 
          }
          else{
               if(distD > distI){
               corrDer();   //corrige a la derecha 
               }
               else{
               corrIzq();
               }
               stopt(10);
          }*/
     adelante(); // un cuadrante
     stopt(10);
    }
    else
        if(distA < 10)
        {
          stopt(10);
          if(distD < 10 && distI > 10) {
            adelante();  
            giroIzquierdat(50);
          }
          else
          if(distI < 10 && distD > 10) {
            adelante();  
            giroDerechat(40);
          }
          else
          if(distI > 10 && distD > 10) {
            adelante();  
            giroDerechat(40);
          }
          else
          if(distD < 10 && distI < 10) {
            verificarSalida();
          }
        
          adelante();
          stopt(10);       
        }
    }    
    }
}       
}