main.cpp

00001 #include "mbed.h"
00002 #include "MotorAPasos.h"
00003 #include "SRF05.h"
00004 //#include "Puerta.h"
00005 
00006 MotorAPasos motor1(D3, D4, D5, D6);
00007 //MotorAPasos motor2(PTC3, PTC4, PTC5, PTC6);
00008 SRF05   uAbajo(D8, D9);        //trigger, echo
00009 SRF05   uArriba(D10, D11);       //trigger, echo
00010 //Puerta  servo(D13);
00011 Serial pc(USBTX, USBRX);
00012 
00013 DigitalIn sArriba(D12);     //1 cuando ya llego
00014 DigitalIn sAbajo(D13);
00015 //DigitalIn IR(D14);     //1 cuando detecta a alguien
00016 //DigitalIn iAbajo(D15);
00017 
00018 int pps=50; //pasos por segundo
00019 int dist=5; //distancia hacia la persona
00020 
00021 int dAbajo, dArriba;
00022 
00023 int main() 
00024 {
00025 //    pc.printf("Ok\n");    
00026     wait(1);
00027 //    motor1.pasosPorSegundo(pps);
00028     while(1) 
00029     {
00030 //        pc.printf("Ok uAbajo %f\n",uAbajo.read());
00031         dAbajo=uAbajo.read();
00032         dArriba=uArriba.read();
00033         pc.printf("uAbajo %d \t uArriba %d\n", dAbajo, dArriba);
00034         if(dAbajo<5)     //si hay alguien en la entrada de abajo
00035         {
00036 
00037             if(sAbajo==1)   //si ya esta abajo
00038                 {
00039                     wait(5);
00040 //                    while(IR==1)    //hay alguien en la puerta
00041 //                    {
00042 //                    }
00043                     wait(5);
00044                     while(sArriba==0)   //mientras no ha llegado sube
00045                     {
00046                         motor1.derecha();
00047                     }
00048                     motor1.stop();
00049                     wait(5);
00050 //                    while(IR==1)   //hay alguien en la puerta
00051 //                    {
00052 //                    }
00053                     wait(5);
00054                 }
00055             if(sAbajo==0)   //si no esta abajo
00056             {
00057 //                if(IR==0)  //si no hay nadie en la puerta de arriba
00058 //                {
00059                     while(sAbajo==0)    //mietras no ha llegado, baja
00060                     {
00061                         motor1.izquierda();
00062                     }
00063                     motor1.stop();
00064 //                    wait(5);
00065 //                }
00066             }
00067         }
00068         
00069         if(dArriba<5)     //si hay alguien en la entrada de arriba
00070         {
00071             if(sArriba==1)   //si ya esta arriba
00072                 {
00073                     wait(5);
00074 //                    while(IR==1)    //hay alguien en la puerta
00075 //                    {
00076 //                    }
00077                     wait(5);
00078                     while(sAbajo==0)   //mientras no ha llegado baja
00079                     {
00080                         motor1.izquierda();
00081                     }
00082                     motor1.stop();
00083                     wait(5);
00084 //                    while(IR==1)   //hay alguien en la puerta
00085 //                    {
00086 //                    }
00087                     wait(5);
00088                 }
00089             if(sArriba==0)   //si no esta arriba
00090             {
00091 //                if(IR==0)  //si no hay nadie en la puerta de abajo
00092 //                {
00093                     while(sArriba==0)    //mietras no ha llegado, sube
00094                     {
00095                         motor1.derecha();
00096                     }
00097                     motor1.stop();
00098 //                    wait(5);
00099 //                }
00100             }
00101         }
00102     }
00103 }