Rampa
Dependencies: MotorAPasos SRF05 mbed
main.cpp@2:85b81b653708, 2016-03-07 (annotated)
- Committer:
- DiegoRivera
- Date:
- Mon Mar 07 06:15:23 2016 +0000
- Revision:
- 2:85b81b653708
- Parent:
- 1:d8b36fb1532f
sin wait cuando esta del lado contrario
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 | 1:d8b36fb1532f | 6 | MotorAPasos motor1(D3, D4, D5, D6); |
DiegoRivera | 0:8e6feefe4208 | 7 | //MotorAPasos motor2(PTC3, PTC4, PTC5, PTC6); |
DiegoRivera | 1:d8b36fb1532f | 8 | SRF05 uAbajo(D8, D9); //trigger, echo |
DiegoRivera | 1:d8b36fb1532f | 9 | SRF05 uArriba(D10, D11); //trigger, echo |
DiegoRivera | 0:8e6feefe4208 | 10 | //Puerta servo(D13); |
DiegoRivera | 1:d8b36fb1532f | 11 | Serial pc(USBTX, USBRX); |
DiegoRivera | 0:8e6feefe4208 | 12 | |
DiegoRivera | 0:8e6feefe4208 | 13 | DigitalIn sArriba(D12); //1 cuando ya llego |
DiegoRivera | 0:8e6feefe4208 | 14 | DigitalIn sAbajo(D13); |
DiegoRivera | 1:d8b36fb1532f | 15 | //DigitalIn IR(D14); //1 cuando detecta a alguien |
DiegoRivera | 0:8e6feefe4208 | 16 | //DigitalIn iAbajo(D15); |
DiegoRivera | 0:8e6feefe4208 | 17 | |
DiegoRivera | 0:8e6feefe4208 | 18 | int pps=50; //pasos por segundo |
DiegoRivera | 0:8e6feefe4208 | 19 | int dist=5; //distancia hacia la persona |
DiegoRivera | 0:8e6feefe4208 | 20 | |
DiegoRivera | 1:d8b36fb1532f | 21 | int dAbajo, dArriba; |
DiegoRivera | 1:d8b36fb1532f | 22 | |
DiegoRivera | 0:8e6feefe4208 | 23 | int main() |
DiegoRivera | 0:8e6feefe4208 | 24 | { |
DiegoRivera | 1:d8b36fb1532f | 25 | // pc.printf("Ok\n"); |
DiegoRivera | 1:d8b36fb1532f | 26 | wait(1); |
DiegoRivera | 1:d8b36fb1532f | 27 | // motor1.pasosPorSegundo(pps); |
DiegoRivera | 0:8e6feefe4208 | 28 | while(1) |
DiegoRivera | 0:8e6feefe4208 | 29 | { |
DiegoRivera | 1:d8b36fb1532f | 30 | // pc.printf("Ok uAbajo %f\n",uAbajo.read()); |
DiegoRivera | 1:d8b36fb1532f | 31 | dAbajo=uAbajo.read(); |
DiegoRivera | 1:d8b36fb1532f | 32 | dArriba=uArriba.read(); |
DiegoRivera | 1:d8b36fb1532f | 33 | pc.printf("uAbajo %d \t uArriba %d\n", dAbajo, dArriba); |
DiegoRivera | 1:d8b36fb1532f | 34 | if(dAbajo<5) //si hay alguien en la entrada de abajo |
DiegoRivera | 0:8e6feefe4208 | 35 | { |
DiegoRivera | 1:d8b36fb1532f | 36 | |
DiegoRivera | 0:8e6feefe4208 | 37 | if(sAbajo==1) //si ya esta abajo |
DiegoRivera | 0:8e6feefe4208 | 38 | { |
DiegoRivera | 0:8e6feefe4208 | 39 | wait(5); |
DiegoRivera | 1:d8b36fb1532f | 40 | // while(IR==1) //hay alguien en la puerta |
DiegoRivera | 1:d8b36fb1532f | 41 | // { |
DiegoRivera | 1:d8b36fb1532f | 42 | // } |
DiegoRivera | 0:8e6feefe4208 | 43 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 44 | while(sArriba==0) //mientras no ha llegado sube |
DiegoRivera | 0:8e6feefe4208 | 45 | { |
DiegoRivera | 0:8e6feefe4208 | 46 | motor1.derecha(); |
DiegoRivera | 0:8e6feefe4208 | 47 | } |
DiegoRivera | 0:8e6feefe4208 | 48 | motor1.stop(); |
DiegoRivera | 0:8e6feefe4208 | 49 | wait(5); |
DiegoRivera | 1:d8b36fb1532f | 50 | // while(IR==1) //hay alguien en la puerta |
DiegoRivera | 1:d8b36fb1532f | 51 | // { |
DiegoRivera | 1:d8b36fb1532f | 52 | // } |
DiegoRivera | 0:8e6feefe4208 | 53 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 54 | } |
DiegoRivera | 0:8e6feefe4208 | 55 | if(sAbajo==0) //si no esta abajo |
DiegoRivera | 0:8e6feefe4208 | 56 | { |
DiegoRivera | 1:d8b36fb1532f | 57 | // if(IR==0) //si no hay nadie en la puerta de arriba |
DiegoRivera | 1:d8b36fb1532f | 58 | // { |
DiegoRivera | 0:8e6feefe4208 | 59 | while(sAbajo==0) //mietras no ha llegado, baja |
DiegoRivera | 0:8e6feefe4208 | 60 | { |
DiegoRivera | 0:8e6feefe4208 | 61 | motor1.izquierda(); |
DiegoRivera | 0:8e6feefe4208 | 62 | } |
DiegoRivera | 0:8e6feefe4208 | 63 | motor1.stop(); |
DiegoRivera | 2:85b81b653708 | 64 | // wait(5); |
DiegoRivera | 1:d8b36fb1532f | 65 | // } |
DiegoRivera | 0:8e6feefe4208 | 66 | } |
DiegoRivera | 0:8e6feefe4208 | 67 | } |
DiegoRivera | 0:8e6feefe4208 | 68 | |
DiegoRivera | 1:d8b36fb1532f | 69 | if(dArriba<5) //si hay alguien en la entrada de arriba |
DiegoRivera | 0:8e6feefe4208 | 70 | { |
DiegoRivera | 0:8e6feefe4208 | 71 | if(sArriba==1) //si ya esta arriba |
DiegoRivera | 0:8e6feefe4208 | 72 | { |
DiegoRivera | 0:8e6feefe4208 | 73 | wait(5); |
DiegoRivera | 1:d8b36fb1532f | 74 | // while(IR==1) //hay alguien en la puerta |
DiegoRivera | 1:d8b36fb1532f | 75 | // { |
DiegoRivera | 1:d8b36fb1532f | 76 | // } |
DiegoRivera | 0:8e6feefe4208 | 77 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 78 | while(sAbajo==0) //mientras no ha llegado baja |
DiegoRivera | 0:8e6feefe4208 | 79 | { |
DiegoRivera | 0:8e6feefe4208 | 80 | motor1.izquierda(); |
DiegoRivera | 0:8e6feefe4208 | 81 | } |
DiegoRivera | 0:8e6feefe4208 | 82 | motor1.stop(); |
DiegoRivera | 0:8e6feefe4208 | 83 | wait(5); |
DiegoRivera | 1:d8b36fb1532f | 84 | // while(IR==1) //hay alguien en la puerta |
DiegoRivera | 1:d8b36fb1532f | 85 | // { |
DiegoRivera | 1:d8b36fb1532f | 86 | // } |
DiegoRivera | 0:8e6feefe4208 | 87 | wait(5); |
DiegoRivera | 0:8e6feefe4208 | 88 | } |
DiegoRivera | 0:8e6feefe4208 | 89 | if(sArriba==0) //si no esta arriba |
DiegoRivera | 0:8e6feefe4208 | 90 | { |
DiegoRivera | 1:d8b36fb1532f | 91 | // if(IR==0) //si no hay nadie en la puerta de abajo |
DiegoRivera | 1:d8b36fb1532f | 92 | // { |
DiegoRivera | 0:8e6feefe4208 | 93 | while(sArriba==0) //mietras no ha llegado, sube |
DiegoRivera | 0:8e6feefe4208 | 94 | { |
DiegoRivera | 0:8e6feefe4208 | 95 | motor1.derecha(); |
DiegoRivera | 0:8e6feefe4208 | 96 | } |
DiegoRivera | 0:8e6feefe4208 | 97 | motor1.stop(); |
DiegoRivera | 2:85b81b653708 | 98 | // wait(5); |
DiegoRivera | 1:d8b36fb1532f | 99 | // } |
DiegoRivera | 0:8e6feefe4208 | 100 | } |
DiegoRivera | 0:8e6feefe4208 | 101 | } |
DiegoRivera | 0:8e6feefe4208 | 102 | } |
DiegoRivera | 0:8e6feefe4208 | 103 | } |