Rampa
Dependencies: MotorAPasos SRF05 mbed
main.cpp@0:8e6feefe4208, 2016-03-04 (annotated)
- Committer:
- DiegoRivera
- Date:
- Fri Mar 04 18:30:02 2016 +0000
- Revision:
- 0:8e6feefe4208
- Child:
- 1:d8b36fb1532f
Rampa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DiegoRivera | 0:8e6feefe4208 | 1 | #include "mbed.h" |
DiegoRivera | 0:8e6feefe4208 | 2 | #include "MotorAPasos.h" |
DiegoRivera | 0:8e6feefe4208 | 3 | #include "SRF05.h" |
DiegoRivera | 0:8e6feefe4208 | 4 | //#include "Puerta.h" |
DiegoRivera | 0:8e6feefe4208 | 5 | |
DiegoRivera | 0:8e6feefe4208 | 6 | MotorAPasos motor1(D2, D3, D4, D5); |
DiegoRivera | 0:8e6feefe4208 | 7 | //MotorAPasos motor2(PTC3, PTC4, PTC5, PTC6); |
DiegoRivera | 0:8e6feefe4208 | 8 | SRF05 uArriba(D8, D9); //trigger, echo |
DiegoRivera | 0:8e6feefe4208 | 9 | SRF05 uAbajo(D10, D11); //trigger, echo |
DiegoRivera | 0:8e6feefe4208 | 10 | //Puerta servo(D13); |
DiegoRivera | 0:8e6feefe4208 | 11 | |
DiegoRivera | 0:8e6feefe4208 | 12 | DigitalIn sArriba(D12); //1 cuando ya llego |
DiegoRivera | 0:8e6feefe4208 | 13 | DigitalIn sAbajo(D13); |
DiegoRivera | 0:8e6feefe4208 | 14 | DigitalIn IR(D14); //1 cuando detecta a alguien |
DiegoRivera | 0:8e6feefe4208 | 15 | //DigitalIn iAbajo(D15); |
DiegoRivera | 0:8e6feefe4208 | 16 | |
DiegoRivera | 0:8e6feefe4208 | 17 | int pps=50; //pasos por segundo |
DiegoRivera | 0:8e6feefe4208 | 18 | int dist=5; //distancia hacia la persona |
DiegoRivera | 0:8e6feefe4208 | 19 | |
DiegoRivera | 0:8e6feefe4208 | 20 | int main() |
DiegoRivera | 0:8e6feefe4208 | 21 | { |
DiegoRivera | 0:8e6feefe4208 | 22 | motor1.pasosPorSegundo(pps); |
DiegoRivera | 0:8e6feefe4208 | 23 | while(1) |
DiegoRivera | 0:8e6feefe4208 | 24 | { |
DiegoRivera | 0:8e6feefe4208 | 25 | if(uAbajo<dist) //si hay alguien en la entrada de abajo |
DiegoRivera | 0:8e6feefe4208 | 26 | { |
DiegoRivera | 0:8e6feefe4208 | 27 | if(sAbajo==1) //si ya esta abajo |
DiegoRivera | 0:8e6feefe4208 | 28 | { |
DiegoRivera | 0:8e6feefe4208 | 29 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 30 | while(IR==1) //hay alguien en la puerta |
DiegoRivera | 0:8e6feefe4208 | 31 | { |
DiegoRivera | 0:8e6feefe4208 | 32 | } |
DiegoRivera | 0:8e6feefe4208 | 33 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 34 | while(sArriba==0) //mientras no ha llegado sube |
DiegoRivera | 0:8e6feefe4208 | 35 | { |
DiegoRivera | 0:8e6feefe4208 | 36 | motor1.derecha(); |
DiegoRivera | 0:8e6feefe4208 | 37 | } |
DiegoRivera | 0:8e6feefe4208 | 38 | motor1.stop(); |
DiegoRivera | 0:8e6feefe4208 | 39 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 40 | while(IR==1) //hay alguien en la puerta |
DiegoRivera | 0:8e6feefe4208 | 41 | { |
DiegoRivera | 0:8e6feefe4208 | 42 | } |
DiegoRivera | 0:8e6feefe4208 | 43 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 44 | } |
DiegoRivera | 0:8e6feefe4208 | 45 | if(sAbajo==0) //si no esta abajo |
DiegoRivera | 0:8e6feefe4208 | 46 | { |
DiegoRivera | 0:8e6feefe4208 | 47 | if(IR==0) //si no hay nadie en la puerta de arriba |
DiegoRivera | 0:8e6feefe4208 | 48 | { |
DiegoRivera | 0:8e6feefe4208 | 49 | while(sAbajo==0) //mietras no ha llegado, baja |
DiegoRivera | 0:8e6feefe4208 | 50 | { |
DiegoRivera | 0:8e6feefe4208 | 51 | motor1.izquierda(); |
DiegoRivera | 0:8e6feefe4208 | 52 | } |
DiegoRivera | 0:8e6feefe4208 | 53 | motor1.stop(); |
DiegoRivera | 0:8e6feefe4208 | 54 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 55 | } |
DiegoRivera | 0:8e6feefe4208 | 56 | } |
DiegoRivera | 0:8e6feefe4208 | 57 | } |
DiegoRivera | 0:8e6feefe4208 | 58 | |
DiegoRivera | 0:8e6feefe4208 | 59 | if(uArriba<dist) //si hay alguien en la entrada de arriba |
DiegoRivera | 0:8e6feefe4208 | 60 | { |
DiegoRivera | 0:8e6feefe4208 | 61 | if(sArriba==1) //si ya esta arriba |
DiegoRivera | 0:8e6feefe4208 | 62 | { |
DiegoRivera | 0:8e6feefe4208 | 63 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 64 | while(IR==1) //hay alguien en la puerta |
DiegoRivera | 0:8e6feefe4208 | 65 | { |
DiegoRivera | 0:8e6feefe4208 | 66 | } |
DiegoRivera | 0:8e6feefe4208 | 67 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 68 | while(sAbajo==0) //mientras no ha llegado baja |
DiegoRivera | 0:8e6feefe4208 | 69 | { |
DiegoRivera | 0:8e6feefe4208 | 70 | motor1.izquierda(); |
DiegoRivera | 0:8e6feefe4208 | 71 | } |
DiegoRivera | 0:8e6feefe4208 | 72 | motor1.stop(); |
DiegoRivera | 0:8e6feefe4208 | 73 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 74 | while(IR==1) //hay alguien en la puerta |
DiegoRivera | 0:8e6feefe4208 | 75 | { |
DiegoRivera | 0:8e6feefe4208 | 76 | } |
DiegoRivera | 0:8e6feefe4208 | 77 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 78 | } |
DiegoRivera | 0:8e6feefe4208 | 79 | if(sArriba==0) //si no esta arriba |
DiegoRivera | 0:8e6feefe4208 | 80 | { |
DiegoRivera | 0:8e6feefe4208 | 81 | if(IR==0) //si no hay nadie en la puerta de abajo |
DiegoRivera | 0:8e6feefe4208 | 82 | { |
DiegoRivera | 0:8e6feefe4208 | 83 | while(sArriba==0) //mietras no ha llegado, sube |
DiegoRivera | 0:8e6feefe4208 | 84 | { |
DiegoRivera | 0:8e6feefe4208 | 85 | motor1.derecha(); |
DiegoRivera | 0:8e6feefe4208 | 86 | } |
DiegoRivera | 0:8e6feefe4208 | 87 | motor1.stop(); |
DiegoRivera | 0:8e6feefe4208 | 88 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 89 | } |
DiegoRivera | 0:8e6feefe4208 | 90 | } |
DiegoRivera | 0:8e6feefe4208 | 91 | } |
DiegoRivera | 0:8e6feefe4208 | 92 | } |
DiegoRivera | 0:8e6feefe4208 | 93 | } |