main.cpp

00001 #include "mbed.h"
00002 #include "hcsr04.h"
00003 #include "motordriver.h"
00004 
00005 //Sensores de Distancia 
00006 HCSR04 usensorA(p10,p11);
00007 HCSR04 usensorD(p6,p5);
00008 HCSR04 usensorI(p20,p19);
00009 //Motores
00010 DigitalOut STBY(p15);
00011 PwmOut PWMA(p26);
00012 DigitalOut AIN1(p30);
00013 DigitalOut AIN2(p29);
00014 PwmOut PWMB(p25);
00015 DigitalOut BIN1(p14);
00016 DigitalOut BIN2(p13);
00017 //Buttons
00018 DigitalIn btn1(p16);
00019 InterruptIn btn2(p17);
00020 
00021 //Configuración de interrupciones
00022 //p10,p11,p12,p13
00023 InterruptIn INT0(p22);     //Interrupción externa 0  CHA1
00024 InterruptIn INT1(p21);     //Interrupción externa 1  CHB1
00025 InterruptIn INT2(p23);     //Interrupción externa 2  CHA2
00026 InterruptIn INT3(p24);     //Interrupción externa 3  CHB2
00027 int CNT1, CNT2; //Contadores de posición para encoders
00028 bool flagCHA1, flagCHB1, flagCHA2, flagCHB2;
00029 
00030 unsigned int distA, distD, distI;
00031 
00032 long ti = 0;
00033 void tiempo(double tiempoControlar, double tiempoInicial)
00034 {
00035     double ti_transcu = 0;
00036     double t = clock();
00037     while (tiempoControlar  > ti_transcu)
00038     {
00039        ti_transcu = t - tiempoInicial;
00040        t = clock();
00041     }
00042 }
00043 void adelante(){             
00044     STBY = 1;          
00045     AIN1 = 1;
00046     AIN2 = 0;
00047     PWMA.write(0.32f);
00048     BIN1 = 1;
00049     BIN2 = 0;
00050     PWMB.write(0.34f);
00051 }
00052 void giroDerecha(){            
00053     STBY = 1;        
00054     AIN1 = 1;
00055     AIN2 = 0;
00056     PWMA.write(0.29f);
00057     BIN1 = 0;
00058     BIN2 = 1;
00059     PWMB.write(0.30f);
00060 }
00061 void giroIzquierda(){        
00062     STBY = 1;        
00063     AIN1 = 0;
00064     AIN2 = 1;
00065     PWMA.write(0.29f);
00066     BIN1 = 1;
00067     BIN2 = 0;
00068     PWMB.write(0.30f);
00069 }
00070 /*
00071 void corrDer(){
00072     STBY = 1;        
00073     AIN1 = 1;
00074     AIN2 = 0;
00075     PWMA.write(0.1f);
00076 }
00077 void corrIzq(){
00078     STBY = 1;        
00079     BIN1 = 1;
00080     BIN2 = 0;
00081     PWMB.write(0.05f);
00082 }*/
00083 void adelantet(long tmedir){
00084       ti=clock();
00085       adelante();
00086       tiempo(tmedir,ti);
00087 }  
00088 void giroDerechat(long tmedir){
00089       ti=clock();
00090       giroDerecha();
00091       tiempo(tmedir,ti);
00092 }  
00093 void giroIzquierdat(long tmedir){
00094       ti=clock();
00095       giroIzquierda();
00096       tiempo(tmedir,ti);
00097 }  
00098 void stopt(long tmedir){
00099       ti=clock();
00100       STBY = 0;
00101       tiempo(tmedir,ti);
00102 } 
00103 void verificarSalida()
00104 {
00105     //if(distA < distD && distA < distI)
00106     if(distD >= distI)
00107         giroDerechat(90); // 180 grados
00108     else
00109         if(distI > distD)
00110            giroIzquierdat(90); // 180 grados
00111 }
00112 void btn2_recorrido() {
00113     STBY = 0;
00114     stopt(500);
00115 }
00116 
00117 int row = 10;
00118 int col = 10;
00119 int pos;
00120 int posicion(int row, int col)
00121 {
00122     if(row > 5){
00123         int nrow = 0;
00124         nrow = (row - 6);
00125         row = (5 - nrow);
00126     }
00127     if(col > 5){
00128         int ncol = 0;
00129         ncol = (col - 6);
00130         col = (5 - ncol);
00131     }
00132     
00133 return (9 -(row - 1) - (col - 1));
00134 }
00135 
00136 int main(){
00137 
00138 btn2.rise(&btn2_recorrido);//interrupcion para detener  
00139 
00140 while(1){
00141     // recorrer laberinto
00142     if(btn1.read()){  //Detect Switch Pres  
00143     stopt(400); //  espera de 4 s
00144     while(pos != 1){
00145 
00146     distA=usensorA.distance(); wait_ms(90);
00147     distI=usensorI.distance(); wait_ms(90); 
00148     distD=usensorD.distance(); wait_ms(90); 
00149     
00150     pos = posicion(row, col); // buscar meta
00151     
00152     if(distA >= 10) {
00153       if(distD > 10)
00154       {
00155         adelante();  
00156         giroDerechat(40); //90 grados
00157        }
00158         
00159       /*else
00160           if(distD == distI){ 
00161           }
00162           else{
00163                if(distD > distI){
00164                corrDer();   //corrige a la derecha 
00165                }
00166                else{
00167                corrIzq();
00168                }
00169                stopt(10);
00170           }*/
00171      adelante(); // un cuadrante
00172      stopt(10);
00173     }
00174     else
00175         if(distA < 10)
00176         {
00177           stopt(10);
00178           if(distD < 10 && distI > 10) {
00179             adelante();  
00180             giroIzquierdat(50);
00181           }
00182           else
00183           if(distI < 10 && distD > 10) {
00184             adelante();  
00185             giroDerechat(40);
00186           }
00187           else
00188           if(distI > 10 && distD > 10) {
00189             adelante();  
00190             giroDerechat(40);
00191           }
00192           else
00193           if(distD < 10 && distI < 10) {
00194             verificarSalida();
00195           }
00196         
00197           adelante();
00198           stopt(10);       
00199         }
00200     }    
00201     }
00202 }       
00203 }