Diff: main.cpp

Revision:

0:9be8543766b6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 27 20:09:52 2017 +0000
@@ -0,0 +1,116 @@
+#include "mbed.h"
+#include "hcsr04.h"
+
+//Serial pc(USBTX, USBRX);
+//Sensores de Distancia 
+HCSR04  usensorA(p10,p11);
+HCSR04  usensorD(p6,p5);
+HCSR04  usensorI(p20,p19);
+//Motores
+DigitalOut STBY(p15);//p7
+PwmOut PWMA(p26);//p21
+DigitalOut AIN1(p30);//p5
+DigitalOut AIN2(p29);//p6
+PwmOut PWMB(p25);//p22
+DigitalOut BIN1(p14);//p8
+DigitalOut BIN2(p13);//p9
+//Interrupcion
+
+unsigned int distA, distD, distI;
+
+void girarIzquierda()
+{
+    BIN1 = 1;
+    BIN2 = 0;
+    PWMB = 2;
+    AIN1 = 0;
+    AIN2 = 1;
+    PWMA = 2;
+}
+
+void girarDerecha()
+{
+    BIN1 = 0;
+    BIN2 = 1;
+    PWMB = 2;
+    AIN1 = 1;
+    AIN2 = 0;
+    PWMA = 2;
+}
+
+void adelante()
+{
+    BIN1 = 1;
+    BIN2 = 0;
+    PWMB = 2;
+    AIN1 = 1;
+    AIN2 = 0;
+    PWMA = 2;
+}
+
+void verificarSalida()
+{
+    if(distA < distD && distA < distI)
+    {
+        if(distD > distI)
+        {
+            girarDerecha();
+        }
+        else
+            if(distI>distD)
+            {
+                girarIzquierda();    
+            }
+    }
+    else
+    {
+        adelante();    
+    }    
+}
+
+int main()
+{
+   //pc.baud(115200);
+    //pc.printf("Hello World from FRDM-K64F board.\n");
+ 
+    while(1)
+    {
+        usensorI.start();
+        wait_ms(500); 
+        distI=usensorI.get_dist_cm();
+        //pc.printf("IZQUIERDA cm:%ld",distI );
+        
+        usensorA.start();
+        wait_ms(500); 
+        distA=usensorA.get_dist_cm();
+        //pc.printf("ADELANTE cm:%ld",distA );
+        
+        usensorD.start();
+        wait_ms(500); 
+        distD=usensorD.get_dist_cm();
+        //pc.printf("DERECHA cm:%ld",distD );
+        
+        if(distA > 20 && distD>20 && distI>20)
+        {
+            adelante();
+        }
+        if(distA > 10 && distD<10 && distI<10)
+        {
+            adelante();
+        }
+        if(distD>10 && distI<10)
+        {
+            girarDerecha();
+        }
+        if(distI > 10 && distD<10 && distA<10)
+        {
+            girarIzquierda();
+        }
+        if(distA < 10 && distD<10 && distI<10)
+        {
+            verificarSalida();
+        }
+          
+    }
+}
+