Programación Ascensor Tiempo Real
Dependencies: DebouncedIn SRF05 mbed
ascensor.cpp@0:cbc582265222, 2017-06-07 (annotated)
- Committer:
- AaronGonzalez
- Date:
- Wed Jun 07 19:36:48 2017 +0000
- Revision:
- 0:cbc582265222
Ascensor Tiempo Real
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AaronGonzalez | 0:cbc582265222 | 1 | #include "mbed.h" |
AaronGonzalez | 0:cbc582265222 | 2 | #include "SRF05.h" |
AaronGonzalez | 0:cbc582265222 | 3 | #include "DebouncedIn.h" |
AaronGonzalez | 0:cbc582265222 | 4 | |
AaronGonzalez | 0:cbc582265222 | 5 | Serial pc(USBTX, USBRX); |
AaronGonzalez | 0:cbc582265222 | 6 | |
AaronGonzalez | 0:cbc582265222 | 7 | // Declaración de pines para las puertas |
AaronGonzalez | 0:cbc582265222 | 8 | // a partir de entradas analógicas |
AaronGonzalez | 0:cbc582265222 | 9 | PwmOut servo1(PTB0); // entrada analógica servo 1 - listo |
AaronGonzalez | 0:cbc582265222 | 10 | PwmOut servo2(PTB1); // entrada analógica servo 2 - listo |
AaronGonzalez | 0:cbc582265222 | 11 | PwmOut servo3(PTB2); // sentrada analógica servo 3 - listo |
AaronGonzalez | 0:cbc582265222 | 12 | |
AaronGonzalez | 0:cbc582265222 | 13 | int STOP=0; // estado detenido inicializado en 0 |
AaronGonzalez | 0:cbc582265222 | 14 | |
AaronGonzalez | 0:cbc582265222 | 15 | //servo1.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 16 | //servo2.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 17 | //servo3.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 18 | //servo1.pulsewidth(0.00055); |
AaronGonzalez | 0:cbc582265222 | 19 | //servo2.pulsewidth(0.0022); |
AaronGonzalez | 0:cbc582265222 | 20 | //servo3.pulsewidth(0.0022); |
AaronGonzalez | 0:cbc582265222 | 21 | |
AaronGonzalez | 0:cbc582265222 | 22 | // Declaración de pines de los motores |
AaronGonzalez | 0:cbc582265222 | 23 | //DigitalOut motor1(PTC16); |
AaronGonzalez | 0:cbc582265222 | 24 | //DigitalOut motor2(PTC13); |
AaronGonzalez | 0:cbc582265222 | 25 | //DigitalOut enable(PTB3); |
AaronGonzalez | 0:cbc582265222 | 26 | |
AaronGonzalez | 0:cbc582265222 | 27 | int motor1; |
AaronGonzalez | 0:cbc582265222 | 28 | int motor2; |
AaronGonzalez | 0:cbc582265222 | 29 | |
AaronGonzalez | 0:cbc582265222 | 30 | // Declaración de pines para los botones |
AaronGonzalez | 0:cbc582265222 | 31 | DebouncedIn boton1(PTC11); |
AaronGonzalez | 0:cbc582265222 | 32 | DebouncedIn boton2b(PTC10); |
AaronGonzalez | 0:cbc582265222 | 33 | DebouncedIn boton2s(PTC6); |
AaronGonzalez | 0:cbc582265222 | 34 | DebouncedIn boton3(PTC5); |
AaronGonzalez | 0:cbc582265222 | 35 | DebouncedIn boton1i(PTC4); |
AaronGonzalez | 0:cbc582265222 | 36 | DebouncedIn boton2i(PTC3); |
AaronGonzalez | 0:cbc582265222 | 37 | DebouncedIn boton3i(PTC0); |
AaronGonzalez | 0:cbc582265222 | 38 | DebouncedIn boton_p(PTC7); |
AaronGonzalez | 0:cbc582265222 | 39 | |
AaronGonzalez | 0:cbc582265222 | 40 | // Declaración pines del sensor de ultrasonido |
AaronGonzalez | 0:cbc582265222 | 41 | SRF05 sensor(PTD3,PTD1); |
AaronGonzalez | 0:cbc582265222 | 42 | |
AaronGonzalez | 0:cbc582265222 | 43 | // Declaración pines digitales para los led's |
AaronGonzalez | 0:cbc582265222 | 44 | DigitalOut led1(PTE5); |
AaronGonzalez | 0:cbc582265222 | 45 | DigitalOut led2(PTE4); |
AaronGonzalez | 0:cbc582265222 | 46 | DigitalOut led3(PTE3); |
AaronGonzalez | 0:cbc582265222 | 47 | |
AaronGonzalez | 0:cbc582265222 | 48 | // Declaración pines de salida para el motor principal tipo PWM |
AaronGonzalez | 0:cbc582265222 | 49 | // Entradas tipo PWM Análogas para inversión de giro |
AaronGonzalez | 0:cbc582265222 | 50 | PwmOut giro1(PTC1); |
AaronGonzalez | 0:cbc582265222 | 51 | PwmOut giro2(PTC2); |
AaronGonzalez | 0:cbc582265222 | 52 | DigitalOut enable(PTB3); |
AaronGonzalez | 0:cbc582265222 | 53 | |
AaronGonzalez | 0:cbc582265222 | 54 | //giro1.period_ms(20); |
AaronGonzalez | 0:cbc582265222 | 55 | //giro2.period_ms(20); |
AaronGonzalez | 0:cbc582265222 | 56 | |
AaronGonzalez | 0:cbc582265222 | 57 | //A continuación se describe el número de botones a usar |
AaronGonzalez | 0:cbc582265222 | 58 | // los pisos del ascensor, el botón de paro |
AaronGonzalez | 0:cbc582265222 | 59 | // y las distancias entre los pisos |
AaronGonzalez | 0:cbc582265222 | 60 | int botones[4],pisos[3],paro; |
AaronGonzalez | 0:cbc582265222 | 61 | int bussy=0; |
AaronGonzalez | 0:cbc582265222 | 62 | float distancia; |
AaronGonzalez | 0:cbc582265222 | 63 | float dist3=8.0; |
AaronGonzalez | 0:cbc582265222 | 64 | float dist2=28.0; |
AaronGonzalez | 0:cbc582265222 | 65 | float dist1=47.0; |
AaronGonzalez | 0:cbc582265222 | 66 | |
AaronGonzalez | 0:cbc582265222 | 67 | // Pulsadores |
AaronGonzalez | 0:cbc582265222 | 68 | void pulsadores(void const *args){ |
AaronGonzalez | 0:cbc582265222 | 69 | |
AaronGonzalez | 0:cbc582265222 | 70 | while(1) { |
AaronGonzalez | 0:cbc582265222 | 71 | if (boton_p.read()==1){ // Lectura de boton de paro |
AaronGonzalez | 0:cbc582265222 | 72 | STOP=1; |
AaronGonzalez | 0:cbc582265222 | 73 | paro=1; |
AaronGonzalez | 0:cbc582265222 | 74 | } |
AaronGonzalez | 0:cbc582265222 | 75 | if (boton1.read()==1){ |
AaronGonzalez | 0:cbc582265222 | 76 | botones[0]=1; //luego irá al piso 1 |
AaronGonzalez | 0:cbc582265222 | 77 | } |
AaronGonzalez | 0:cbc582265222 | 78 | if (boton2b.read()==1){ |
AaronGonzalez | 0:cbc582265222 | 79 | botones[1]=1; //luego irá al piso 1 |
AaronGonzalez | 0:cbc582265222 | 80 | } |
AaronGonzalez | 0:cbc582265222 | 81 | if (boton2s.read()==1){ |
AaronGonzalez | 0:cbc582265222 | 82 | botones[2]=1; //luego irá al piso 1 |
AaronGonzalez | 0:cbc582265222 | 83 | } |
AaronGonzalez | 0:cbc582265222 | 84 | if (boton3.read()==1){ |
AaronGonzalez | 0:cbc582265222 | 85 | botones[3]=1; //luego irá al piso 1 |
AaronGonzalez | 0:cbc582265222 | 86 | } |
AaronGonzalez | 0:cbc582265222 | 87 | if (boton1i.read()==1){ |
AaronGonzalez | 0:cbc582265222 | 88 | pisos[0]=1; //luego irá al piso 1 |
AaronGonzalez | 0:cbc582265222 | 89 | } |
AaronGonzalez | 0:cbc582265222 | 90 | if (boton2i.read()==1){ |
AaronGonzalez | 0:cbc582265222 | 91 | pisos[1]=1; //luego irá al piso 2 |
AaronGonzalez | 0:cbc582265222 | 92 | } |
AaronGonzalez | 0:cbc582265222 | 93 | if (boton3i.read()==1){ |
AaronGonzalez | 0:cbc582265222 | 94 | pisos[2]=1; //luego irá al piso 3 |
AaronGonzalez | 0:cbc582265222 | 95 | } |
AaronGonzalez | 0:cbc582265222 | 96 | // Thread::wait(1000.0*0.02); |
AaronGonzalez | 0:cbc582265222 | 97 | } |
AaronGonzalez | 0:cbc582265222 | 98 | } |
AaronGonzalez | 0:cbc582265222 | 99 | |
AaronGonzalez | 0:cbc582265222 | 100 | void sensar(void const *args){ |
AaronGonzalez | 0:cbc582265222 | 101 | |
AaronGonzalez | 0:cbc582265222 | 102 | while(1) { |
AaronGonzalez | 0:cbc582265222 | 103 | |
AaronGonzalez | 0:cbc582265222 | 104 | distancia=sensor.read(); //sensar |
AaronGonzalez | 0:cbc582265222 | 105 | |
AaronGonzalez | 0:cbc582265222 | 106 | //Thread::wait(1000.0*0.02); |
AaronGonzalez | 0:cbc582265222 | 107 | |
AaronGonzalez | 0:cbc582265222 | 108 | distancia=floor(distancia); |
AaronGonzalez | 0:cbc582265222 | 109 | } |
AaronGonzalez | 0:cbc582265222 | 110 | } |
AaronGonzalez | 0:cbc582265222 | 111 | void subir(void){ |
AaronGonzalez | 0:cbc582265222 | 112 | |
AaronGonzalez | 0:cbc582265222 | 113 | if(motor2==0){ |
AaronGonzalez | 0:cbc582265222 | 114 | giro1.period_ms(20); |
AaronGonzalez | 0:cbc582265222 | 115 | giro2.period_ms(20); |
AaronGonzalez | 0:cbc582265222 | 116 | enable=1; |
AaronGonzalez | 0:cbc582265222 | 117 | motor1=1; |
AaronGonzalez | 0:cbc582265222 | 118 | |
AaronGonzalez | 0:cbc582265222 | 119 | //giro1=1; |
AaronGonzalez | 0:cbc582265222 | 120 | //motor2=0; |
AaronGonzalez | 0:cbc582265222 | 121 | //giro2=0; |
AaronGonzalez | 0:cbc582265222 | 122 | |
AaronGonzalez | 0:cbc582265222 | 123 | giro1.pulsewidth_ms(15); |
AaronGonzalez | 0:cbc582265222 | 124 | giro2.pulsewidth_ms(0); |
AaronGonzalez | 0:cbc582265222 | 125 | } |
AaronGonzalez | 0:cbc582265222 | 126 | } |
AaronGonzalez | 0:cbc582265222 | 127 | void bajar(void){ |
AaronGonzalez | 0:cbc582265222 | 128 | |
AaronGonzalez | 0:cbc582265222 | 129 | if(motor1==0){ |
AaronGonzalez | 0:cbc582265222 | 130 | giro1.period_ms(20); |
AaronGonzalez | 0:cbc582265222 | 131 | giro2.period_ms(20); |
AaronGonzalez | 0:cbc582265222 | 132 | enable=1; |
AaronGonzalez | 0:cbc582265222 | 133 | motor1=0; |
AaronGonzalez | 0:cbc582265222 | 134 | |
AaronGonzalez | 0:cbc582265222 | 135 | //giro1=0; |
AaronGonzalez | 0:cbc582265222 | 136 | //motor2=1; |
AaronGonzalez | 0:cbc582265222 | 137 | //giro2=1; |
AaronGonzalez | 0:cbc582265222 | 138 | |
AaronGonzalez | 0:cbc582265222 | 139 | giro1.pulsewidth_ms(0); |
AaronGonzalez | 0:cbc582265222 | 140 | giro2.pulsewidth_ms(8); |
AaronGonzalez | 0:cbc582265222 | 141 | } |
AaronGonzalez | 0:cbc582265222 | 142 | } |
AaronGonzalez | 0:cbc582265222 | 143 | void stop(void){ |
AaronGonzalez | 0:cbc582265222 | 144 | |
AaronGonzalez | 0:cbc582265222 | 145 | giro1.period_ms(20); |
AaronGonzalez | 0:cbc582265222 | 146 | giro2.period_ms(20); |
AaronGonzalez | 0:cbc582265222 | 147 | enable=0; |
AaronGonzalez | 0:cbc582265222 | 148 | motor1=0; |
AaronGonzalez | 0:cbc582265222 | 149 | |
AaronGonzalez | 0:cbc582265222 | 150 | //giro1=0; |
AaronGonzalez | 0:cbc582265222 | 151 | //motor2=0; |
AaronGonzalez | 0:cbc582265222 | 152 | //giro2=0; |
AaronGonzalez | 0:cbc582265222 | 153 | |
AaronGonzalez | 0:cbc582265222 | 154 | enable=0; |
AaronGonzalez | 0:cbc582265222 | 155 | giro1.pulsewidth_ms(0); |
AaronGonzalez | 0:cbc582265222 | 156 | giro2.pulsewidth_ms(0); |
AaronGonzalez | 0:cbc582265222 | 157 | } |
AaronGonzalez | 0:cbc582265222 | 158 | |
AaronGonzalez | 0:cbc582265222 | 159 | void tercero(void){ |
AaronGonzalez | 0:cbc582265222 | 160 | |
AaronGonzalez | 0:cbc582265222 | 161 | bussy=1; |
AaronGonzalez | 0:cbc582265222 | 162 | botones[3]=0; |
AaronGonzalez | 0:cbc582265222 | 163 | pisos[2]=0; |
AaronGonzalez | 0:cbc582265222 | 164 | servo1.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 165 | servo1.pulsewidth(0.0008); |
AaronGonzalez | 0:cbc582265222 | 166 | servo2.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 167 | servo2.pulsewidth(0.0055); |
AaronGonzalez | 0:cbc582265222 | 168 | servo3.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 169 | servo3.pulsewidth(0.0055); |
AaronGonzalez | 0:cbc582265222 | 170 | |
AaronGonzalez | 0:cbc582265222 | 171 | int flag1=0,flag2=0; |
AaronGonzalez | 0:cbc582265222 | 172 | subir(); |
AaronGonzalez | 0:cbc582265222 | 173 | |
AaronGonzalez | 0:cbc582265222 | 174 | while(1){ |
AaronGonzalez | 0:cbc582265222 | 175 | |
AaronGonzalez | 0:cbc582265222 | 176 | if (botones[2]==1 && distancia>=dist2){ |
AaronGonzalez | 0:cbc582265222 | 177 | flag2=1; |
AaronGonzalez | 0:cbc582265222 | 178 | } |
AaronGonzalez | 0:cbc582265222 | 179 | |
AaronGonzalez | 0:cbc582265222 | 180 | if (flag2==1 && distancia==dist2 && flag1==0){ |
AaronGonzalez | 0:cbc582265222 | 181 | stop(); |
AaronGonzalez | 0:cbc582265222 | 182 | servo2.pulsewidth(0.0008); |
AaronGonzalez | 0:cbc582265222 | 183 | wait(5); |
AaronGonzalez | 0:cbc582265222 | 184 | servo2.pulsewidth(0.0055); |
AaronGonzalez | 0:cbc582265222 | 185 | flag1=1; |
AaronGonzalez | 0:cbc582265222 | 186 | subir(); |
AaronGonzalez | 0:cbc582265222 | 187 | botones[2]=0; |
AaronGonzalez | 0:cbc582265222 | 188 | } |
AaronGonzalez | 0:cbc582265222 | 189 | |
AaronGonzalez | 0:cbc582265222 | 190 | if (botones[1]==1){ |
AaronGonzalez | 0:cbc582265222 | 191 | pisos[1]=1; // va luego para el piso 2 |
AaronGonzalez | 0:cbc582265222 | 192 | } |
AaronGonzalez | 0:cbc582265222 | 193 | |
AaronGonzalez | 0:cbc582265222 | 194 | if (distancia<=dist3){ |
AaronGonzalez | 0:cbc582265222 | 195 | stop(); |
AaronGonzalez | 0:cbc582265222 | 196 | servo3.pulsewidth(0.0008); |
AaronGonzalez | 0:cbc582265222 | 197 | wait(5); |
AaronGonzalez | 0:cbc582265222 | 198 | servo3.pulsewidth(0.0055); |
AaronGonzalez | 0:cbc582265222 | 199 | break; |
AaronGonzalez | 0:cbc582265222 | 200 | } |
AaronGonzalez | 0:cbc582265222 | 201 | } |
AaronGonzalez | 0:cbc582265222 | 202 | bussy=0; |
AaronGonzalez | 0:cbc582265222 | 203 | } |
AaronGonzalez | 0:cbc582265222 | 204 | |
AaronGonzalez | 0:cbc582265222 | 205 | void segundo(void){ |
AaronGonzalez | 0:cbc582265222 | 206 | |
AaronGonzalez | 0:cbc582265222 | 207 | botones[1]=0; |
AaronGonzalez | 0:cbc582265222 | 208 | botones[2]=0; |
AaronGonzalez | 0:cbc582265222 | 209 | pisos[1]=0; |
AaronGonzalez | 0:cbc582265222 | 210 | bussy=1; |
AaronGonzalez | 0:cbc582265222 | 211 | servo1.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 212 | servo1.pulsewidth(0.0008); |
AaronGonzalez | 0:cbc582265222 | 213 | servo2.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 214 | servo2.pulsewidth(0.0055); |
AaronGonzalez | 0:cbc582265222 | 215 | servo3.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 216 | servo3.pulsewidth(0.0055); |
AaronGonzalez | 0:cbc582265222 | 217 | |
AaronGonzalez | 0:cbc582265222 | 218 | if (distancia>(dist2+2.0)){ |
AaronGonzalez | 0:cbc582265222 | 219 | subir(); |
AaronGonzalez | 0:cbc582265222 | 220 | } |
AaronGonzalez | 0:cbc582265222 | 221 | else{ |
AaronGonzalez | 0:cbc582265222 | 222 | |
AaronGonzalez | 0:cbc582265222 | 223 | if (distancia<(dist2-2.0)){ |
AaronGonzalez | 0:cbc582265222 | 224 | bajar();} |
AaronGonzalez | 0:cbc582265222 | 225 | } |
AaronGonzalez | 0:cbc582265222 | 226 | |
AaronGonzalez | 0:cbc582265222 | 227 | while(1){ |
AaronGonzalez | 0:cbc582265222 | 228 | if (distancia==dist2){ |
AaronGonzalez | 0:cbc582265222 | 229 | stop(); |
AaronGonzalez | 0:cbc582265222 | 230 | servo2.pulsewidth(0.0008); |
AaronGonzalez | 0:cbc582265222 | 231 | wait(5); |
AaronGonzalez | 0:cbc582265222 | 232 | servo2.pulsewidth(0.0055); |
AaronGonzalez | 0:cbc582265222 | 233 | break; |
AaronGonzalez | 0:cbc582265222 | 234 | } |
AaronGonzalez | 0:cbc582265222 | 235 | } |
AaronGonzalez | 0:cbc582265222 | 236 | bussy=0; |
AaronGonzalez | 0:cbc582265222 | 237 | } |
AaronGonzalez | 0:cbc582265222 | 238 | |
AaronGonzalez | 0:cbc582265222 | 239 | void primero(void){ |
AaronGonzalez | 0:cbc582265222 | 240 | |
AaronGonzalez | 0:cbc582265222 | 241 | botones[0]=0; |
AaronGonzalez | 0:cbc582265222 | 242 | pisos[0]=0; |
AaronGonzalez | 0:cbc582265222 | 243 | bussy=1; |
AaronGonzalez | 0:cbc582265222 | 244 | servo1.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 245 | servo1.pulsewidth(0.0008); |
AaronGonzalez | 0:cbc582265222 | 246 | servo2.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 247 | servo2.pulsewidth(0.0055); |
AaronGonzalez | 0:cbc582265222 | 248 | servo3.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 249 | servo3.pulsewidth(0.0055); |
AaronGonzalez | 0:cbc582265222 | 250 | int flag1=0,flag2=0; |
AaronGonzalez | 0:cbc582265222 | 251 | bajar(); |
AaronGonzalez | 0:cbc582265222 | 252 | |
AaronGonzalez | 0:cbc582265222 | 253 | while(1){ |
AaronGonzalez | 0:cbc582265222 | 254 | |
AaronGonzalez | 0:cbc582265222 | 255 | if (botones[1]==1 && distancia<=dist2){ |
AaronGonzalez | 0:cbc582265222 | 256 | flag2=1; |
AaronGonzalez | 0:cbc582265222 | 257 | } |
AaronGonzalez | 0:cbc582265222 | 258 | if (flag2==1 && distancia==dist2 && flag1==0){ |
AaronGonzalez | 0:cbc582265222 | 259 | stop(); |
AaronGonzalez | 0:cbc582265222 | 260 | servo2.pulsewidth(0.0008); |
AaronGonzalez | 0:cbc582265222 | 261 | wait(5); |
AaronGonzalez | 0:cbc582265222 | 262 | servo2.pulsewidth(0.0055); |
AaronGonzalez | 0:cbc582265222 | 263 | flag1=1; |
AaronGonzalez | 0:cbc582265222 | 264 | bajar(); |
AaronGonzalez | 0:cbc582265222 | 265 | botones[1]=0; |
AaronGonzalez | 0:cbc582265222 | 266 | } |
AaronGonzalez | 0:cbc582265222 | 267 | if(botones[2]==1){ |
AaronGonzalez | 0:cbc582265222 | 268 | pisos[1]=1; |
AaronGonzalez | 0:cbc582265222 | 269 | } |
AaronGonzalez | 0:cbc582265222 | 270 | if (distancia>=dist1){ |
AaronGonzalez | 0:cbc582265222 | 271 | stop(); |
AaronGonzalez | 0:cbc582265222 | 272 | servo1.pulsewidth(0.0022); |
AaronGonzalez | 0:cbc582265222 | 273 | wait(5); |
AaronGonzalez | 0:cbc582265222 | 274 | servo1.pulsewidth(0.0008); |
AaronGonzalez | 0:cbc582265222 | 275 | break; |
AaronGonzalez | 0:cbc582265222 | 276 | } |
AaronGonzalez | 0:cbc582265222 | 277 | } |
AaronGonzalez | 0:cbc582265222 | 278 | bussy=0; |
AaronGonzalez | 0:cbc582265222 | 279 | } |
AaronGonzalez | 0:cbc582265222 | 280 | |
AaronGonzalez | 0:cbc582265222 | 281 | void bombillos(void const *args){ |
AaronGonzalez | 0:cbc582265222 | 282 | |
AaronGonzalez | 0:cbc582265222 | 283 | while(1){ |
AaronGonzalez | 0:cbc582265222 | 284 | |
AaronGonzalez | 0:cbc582265222 | 285 | if (motor2==1){ |
AaronGonzalez | 0:cbc582265222 | 286 | if (distancia>=dist3 && distancia<dist2){ |
AaronGonzalez | 0:cbc582265222 | 287 | led3=1; |
AaronGonzalez | 0:cbc582265222 | 288 | led2=0; |
AaronGonzalez | 0:cbc582265222 | 289 | led1=0; |
AaronGonzalez | 0:cbc582265222 | 290 | } |
AaronGonzalez | 0:cbc582265222 | 291 | if (distancia>=dist2 && distancia<dist1){ |
AaronGonzalez | 0:cbc582265222 | 292 | led3=0; |
AaronGonzalez | 0:cbc582265222 | 293 | led2=1; |
AaronGonzalez | 0:cbc582265222 | 294 | led1=0; |
AaronGonzalez | 0:cbc582265222 | 295 | } |
AaronGonzalez | 0:cbc582265222 | 296 | if (distancia>=dist1){ |
AaronGonzalez | 0:cbc582265222 | 297 | led3=0; |
AaronGonzalez | 0:cbc582265222 | 298 | led2=0; |
AaronGonzalez | 0:cbc582265222 | 299 | led1=1; |
AaronGonzalez | 0:cbc582265222 | 300 | } |
AaronGonzalez | 0:cbc582265222 | 301 | } |
AaronGonzalez | 0:cbc582265222 | 302 | if (motor1==1 || (motor1==0 && motor2==0)){ |
AaronGonzalez | 0:cbc582265222 | 303 | |
AaronGonzalez | 0:cbc582265222 | 304 | if (distancia<=dist1 && distancia>dist2){ |
AaronGonzalez | 0:cbc582265222 | 305 | led3=0; |
AaronGonzalez | 0:cbc582265222 | 306 | led2=0; |
AaronGonzalez | 0:cbc582265222 | 307 | led1=1; |
AaronGonzalez | 0:cbc582265222 | 308 | } |
AaronGonzalez | 0:cbc582265222 | 309 | if (distancia<=dist2 && distancia>dist3){ |
AaronGonzalez | 0:cbc582265222 | 310 | led3=0; |
AaronGonzalez | 0:cbc582265222 | 311 | led2=1; |
AaronGonzalez | 0:cbc582265222 | 312 | led1=0; |
AaronGonzalez | 0:cbc582265222 | 313 | } |
AaronGonzalez | 0:cbc582265222 | 314 | if (distancia<=dist3){ |
AaronGonzalez | 0:cbc582265222 | 315 | led3=1; |
AaronGonzalez | 0:cbc582265222 | 316 | led2=0; |
AaronGonzalez | 0:cbc582265222 | 317 | led1=0; |
AaronGonzalez | 0:cbc582265222 | 318 | } |
AaronGonzalez | 0:cbc582265222 | 319 | } |
AaronGonzalez | 0:cbc582265222 | 320 | |
AaronGonzalez | 0:cbc582265222 | 321 | //Thread::wait(1000.0*0.02); |
AaronGonzalez | 0:cbc582265222 | 322 | |
AaronGonzalez | 0:cbc582265222 | 323 | } |
AaronGonzalez | 0:cbc582265222 | 324 | } |
AaronGonzalez | 0:cbc582265222 | 325 | |
AaronGonzalez | 0:cbc582265222 | 326 | void parar(void){ |
AaronGonzalez | 0:cbc582265222 | 327 | |
AaronGonzalez | 0:cbc582265222 | 328 | servo1.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 329 | servo1.pulsewidth(0.0008); |
AaronGonzalez | 0:cbc582265222 | 330 | servo2.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 331 | servo2.pulsewidth(0.0055); |
AaronGonzalez | 0:cbc582265222 | 332 | servo3.period(0.025); |
AaronGonzalez | 0:cbc582265222 | 333 | servo3.pulsewidth(0.0055); |
AaronGonzalez | 0:cbc582265222 | 334 | bajar(); |
AaronGonzalez | 0:cbc582265222 | 335 | |
AaronGonzalez | 0:cbc582265222 | 336 | while(1) { |
AaronGonzalez | 0:cbc582265222 | 337 | if (distancia>=dist1){ |
AaronGonzalez | 0:cbc582265222 | 338 | stop(); |
AaronGonzalez | 0:cbc582265222 | 339 | servo1.pulsewidth(0.0008); |
AaronGonzalez | 0:cbc582265222 | 340 | botones[0]=0; |
AaronGonzalez | 0:cbc582265222 | 341 | botones[1]=0; |
AaronGonzalez | 0:cbc582265222 | 342 | botones[2]=0; |
AaronGonzalez | 0:cbc582265222 | 343 | botones[3]=0; |
AaronGonzalez | 0:cbc582265222 | 344 | pisos[0]=0; |
AaronGonzalez | 0:cbc582265222 | 345 | pisos[1]=0; |
AaronGonzalez | 0:cbc582265222 | 346 | pisos[2]=0; |
AaronGonzalez | 0:cbc582265222 | 347 | STOP=0; |
AaronGonzalez | 0:cbc582265222 | 348 | paro=0; |
AaronGonzalez | 0:cbc582265222 | 349 | break; |
AaronGonzalez | 0:cbc582265222 | 350 | } |
AaronGonzalez | 0:cbc582265222 | 351 | } |
AaronGonzalez | 0:cbc582265222 | 352 | } |
AaronGonzalez | 0:cbc582265222 | 353 | |
AaronGonzalez | 0:cbc582265222 | 354 | int main() { |
AaronGonzalez | 0:cbc582265222 | 355 | |
AaronGonzalez | 0:cbc582265222 | 356 | giro1.period_ms(20); |
AaronGonzalez | 0:cbc582265222 | 357 | giro2.period_ms(20); |
AaronGonzalez | 0:cbc582265222 | 358 | giro1.pulsewidth_ms(0); |
AaronGonzalez | 0:cbc582265222 | 359 | giro2.pulsewidth_ms(0); |
AaronGonzalez | 0:cbc582265222 | 360 | |
AaronGonzalez | 0:cbc582265222 | 361 | enable=0; |
AaronGonzalez | 0:cbc582265222 | 362 | motor1=0; |
AaronGonzalez | 0:cbc582265222 | 363 | motor2=0; |
AaronGonzalez | 0:cbc582265222 | 364 | led1=0; |
AaronGonzalez | 0:cbc582265222 | 365 | led2=0; |
AaronGonzalez | 0:cbc582265222 | 366 | led3=0; |
AaronGonzalez | 0:cbc582265222 | 367 | |
AaronGonzalez | 0:cbc582265222 | 368 | //Thread thread2(pulsadores); |
AaronGonzalez | 0:cbc582265222 | 369 | //Thread thread4(sensar); |
AaronGonzalez | 0:cbc582265222 | 370 | //Thread thread5(bombillos); |
AaronGonzalez | 0:cbc582265222 | 371 | |
AaronGonzalez | 0:cbc582265222 | 372 | botones[0]=0; |
AaronGonzalez | 0:cbc582265222 | 373 | botones[1]=0; |
AaronGonzalez | 0:cbc582265222 | 374 | botones[2]=0; |
AaronGonzalez | 0:cbc582265222 | 375 | botones[3]=0; |
AaronGonzalez | 0:cbc582265222 | 376 | pisos[0]=0; |
AaronGonzalez | 0:cbc582265222 | 377 | pisos[1]=0; |
AaronGonzalez | 0:cbc582265222 | 378 | pisos[2]=0; |
AaronGonzalez | 0:cbc582265222 | 379 | |
AaronGonzalez | 0:cbc582265222 | 380 | while(1) { |
AaronGonzalez | 0:cbc582265222 | 381 | if (paro==1 ){ |
AaronGonzalez | 0:cbc582265222 | 382 | botones[0]=0; |
AaronGonzalez | 0:cbc582265222 | 383 | botones[1]=0; |
AaronGonzalez | 0:cbc582265222 | 384 | botones[2]=0; |
AaronGonzalez | 0:cbc582265222 | 385 | botones[3]=0; |
AaronGonzalez | 0:cbc582265222 | 386 | pisos[0]=0; |
AaronGonzalez | 0:cbc582265222 | 387 | pisos[1]=0; |
AaronGonzalez | 0:cbc582265222 | 388 | pisos[2]=0; |
AaronGonzalez | 0:cbc582265222 | 389 | STOP=0; |
AaronGonzalez | 0:cbc582265222 | 390 | paro=0; |
AaronGonzalez | 0:cbc582265222 | 391 | primero(); |
AaronGonzalez | 0:cbc582265222 | 392 | } |
AaronGonzalez | 0:cbc582265222 | 393 | if (STOP==0 && bussy==0){ |
AaronGonzalez | 0:cbc582265222 | 394 | |
AaronGonzalez | 0:cbc582265222 | 395 | if (pisos[0]==1){ |
AaronGonzalez | 0:cbc582265222 | 396 | pisos[0]=0; |
AaronGonzalez | 0:cbc582265222 | 397 | primero(); |
AaronGonzalez | 0:cbc582265222 | 398 | } |
AaronGonzalez | 0:cbc582265222 | 399 | if ( pisos[1]==1){ |
AaronGonzalez | 0:cbc582265222 | 400 | pisos[1]=0; |
AaronGonzalez | 0:cbc582265222 | 401 | segundo(); |
AaronGonzalez | 0:cbc582265222 | 402 | } |
AaronGonzalez | 0:cbc582265222 | 403 | if (pisos[2]==1){ |
AaronGonzalez | 0:cbc582265222 | 404 | pisos[2]=0; |
AaronGonzalez | 0:cbc582265222 | 405 | tercero(); |
AaronGonzalez | 0:cbc582265222 | 406 | } |
AaronGonzalez | 0:cbc582265222 | 407 | if (botones[0]==0 && botones[1]==0 && botones[2]==0 && botones[3]==0){ |
AaronGonzalez | 0:cbc582265222 | 408 | } |
AaronGonzalez | 0:cbc582265222 | 409 | if ( botones[3]==1){ |
AaronGonzalez | 0:cbc582265222 | 410 | botones[3]=0; |
AaronGonzalez | 0:cbc582265222 | 411 | tercero(); |
AaronGonzalez | 0:cbc582265222 | 412 | } |
AaronGonzalez | 0:cbc582265222 | 413 | if (botones[1]==1 || botones[2]==1){ |
AaronGonzalez | 0:cbc582265222 | 414 | botones[1]=0; |
AaronGonzalez | 0:cbc582265222 | 415 | botones[2]=0; |
AaronGonzalez | 0:cbc582265222 | 416 | segundo(); |
AaronGonzalez | 0:cbc582265222 | 417 | } |
AaronGonzalez | 0:cbc582265222 | 418 | if (botones[0]==1 ){ |
AaronGonzalez | 0:cbc582265222 | 419 | botones[0]=0; |
AaronGonzalez | 0:cbc582265222 | 420 | primero(); |
AaronGonzalez | 0:cbc582265222 | 421 | } |
AaronGonzalez | 0:cbc582265222 | 422 | } |
AaronGonzalez | 0:cbc582265222 | 423 | |
AaronGonzalez | 0:cbc582265222 | 424 | //Thread::wait(1000.0*0.2); |
AaronGonzalez | 0:cbc582265222 | 425 | //myled = 0; |
AaronGonzalez | 0:cbc582265222 | 426 | //Thread::wait(1000.0*0.2); |
AaronGonzalez | 0:cbc582265222 | 427 | } |
AaronGonzalez | 0:cbc582265222 | 428 | } |