Diff: main.cpp

Revision:

0:8e6feefe4208

Child:

1:d8b36fb1532f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 04 18:30:02 2016 +0000
@@ -0,0 +1,93 @@
+#include "mbed.h"
+#include "MotorAPasos.h"
+#include "SRF05.h"
+//#include "Puerta.h"
+
+MotorAPasos motor1(D2, D3, D4, D5);
+//MotorAPasos motor2(PTC3, PTC4, PTC5, PTC6);
+SRF05   uArriba(D8, D9);        //trigger, echo
+SRF05   uAbajo(D10, D11);       //trigger, echo
+//Puerta  servo(D13);
+
+DigitalIn sArriba(D12);     //1 cuando ya llego
+DigitalIn sAbajo(D13);
+DigitalIn IR(D14);     //1 cuando detecta a alguien
+//DigitalIn iAbajo(D15);
+
+int pps=50; //pasos por segundo
+int dist=5; //distancia hacia la persona
+
+int main() 
+{
+    motor1.pasosPorSegundo(pps);
+    while(1) 
+    {
+        if(uAbajo<dist)     //si hay alguien en la entrada de abajo
+        {
+            if(sAbajo==1)   //si ya esta abajo
+                {
+                    wait(5);
+                    while(IR==1)    //hay alguien en la puerta
+                    {
+                    }
+                    wait(5);
+                    while(sArriba==0)   //mientras no ha llegado sube
+                    {
+                        motor1.derecha();
+                    }
+                    motor1.stop();
+                    wait(5);
+                    while(IR==1)   //hay alguien en la puerta
+                    {
+                    }
+                    wait(5);
+                }
+            if(sAbajo==0)   //si no esta abajo
+            {
+                if(IR==0)  //si no hay nadie en la puerta de arriba
+                {
+                    while(sAbajo==0)    //mietras no ha llegado, baja
+                    {
+                        motor1.izquierda();
+                    }
+                    motor1.stop();
+                    wait(5);
+                }
+            }
+        }
+        
+        if(uArriba<dist)     //si hay alguien en la entrada de arriba
+        {
+            if(sArriba==1)   //si ya esta arriba
+                {
+                    wait(5);
+                    while(IR==1)    //hay alguien en la puerta
+                    {
+                    }
+                    wait(5);
+                    while(sAbajo==0)   //mientras no ha llegado baja
+                    {
+                        motor1.izquierda();
+                    }
+                    motor1.stop();
+                    wait(5);
+                    while(IR==1)   //hay alguien en la puerta
+                    {
+                    }
+                    wait(5);
+                }
+            if(sArriba==0)   //si no esta arriba
+            {
+                if(IR==0)  //si no hay nadie en la puerta de abajo
+                {
+                    while(sArriba==0)    //mietras no ha llegado, sube
+                    {
+                        motor1.derecha();
+                    }
+                    motor1.stop();
+                    wait(5);
+                }
+            }
+        }
+    }
+}