Rampa
Dependencies: MotorAPasos SRF05 mbed
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); + } + } + } + } +}