main.cpp

00001 #include "mbed.h"
00002 #include "hcsr04.h"
00003 
00004 //Serial pc(USBTX, USBRX);
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);//p7
00011 PwmOut PWMA(p26);//p21
00012 DigitalOut AIN1(p30);//p5
00013 DigitalOut AIN2(p29);//p6
00014 PwmOut PWMB(p25);//p22
00015 DigitalOut BIN1(p14);//p8
00016 DigitalOut BIN2(p13);//p9
00017 //Interrupcion
00018 
00019 unsigned int distA, distD, distI;
00020 
00021 void girarIzquierda()
00022 {
00023     BIN1 = 1;
00024     BIN2 = 0;
00025     PWMB = 2;
00026     AIN1 = 0;
00027     AIN2 = 1;
00028     PWMA = 2;
00029 }
00030 
00031 void girarDerecha()
00032 {
00033     BIN1 = 0;
00034     BIN2 = 1;
00035     PWMB = 2;
00036     AIN1 = 1;
00037     AIN2 = 0;
00038     PWMA = 2;
00039 }
00040 
00041 void adelante()
00042 {
00043     BIN1 = 1;
00044     BIN2 = 0;
00045     PWMB = 2;
00046     AIN1 = 1;
00047     AIN2 = 0;
00048     PWMA = 2;
00049 }
00050 
00051 void verificarSalida()
00052 {
00053     if(distA < distD && distA < distI)
00054     {
00055         if(distD > distI)
00056         {
00057             girarDerecha();
00058         }
00059         else
00060             if(distI>distD)
00061             {
00062                 girarIzquierda();    
00063             }
00064     }
00065     else
00066     {
00067         adelante();    
00068     }    
00069 }
00070 
00071 int main()
00072 {
00073    //pc.baud(115200);
00074     //pc.printf("Hello World from FRDM-K64F board.\n");
00075  
00076     while(1)
00077     {
00078         usensorI.start();
00079         wait_ms(500); 
00080         distI=usensorI.get_dist_cm();
00081         //pc.printf("IZQUIERDA cm:%ld",distI );
00082         
00083         usensorA.start();
00084         wait_ms(500); 
00085         distA=usensorA.get_dist_cm();
00086         //pc.printf("ADELANTE cm:%ld",distA );
00087         
00088         usensorD.start();
00089         wait_ms(500); 
00090         distD=usensorD.get_dist_cm();
00091         //pc.printf("DERECHA cm:%ld",distD );
00092         
00093         if(distA > 20 && distD>20 && distI>20)
00094         {
00095             adelante();
00096         }
00097         if(distA > 10 && distD<10 && distI<10)
00098         {
00099             adelante();
00100         }
00101         if(distD>10 && distI<10)
00102         {
00103             girarDerecha();
00104         }
00105         if(distI > 10 && distD<10 && distA<10)
00106         {
00107             girarIzquierda();
00108         }
00109         if(distA < 10 && distD<10 && distI<10)
00110         {
00111             verificarSalida();
00112         }
00113           
00114     }
00115 }
00116