ascensor.cpp

00001 #include "mbed.h"
00002 #include "SRF05.h"
00003 #include "DebouncedIn.h"
00004 
00005 Serial pc(USBTX, USBRX);
00006 
00007 // Declaración de pines para las puertas
00008 // a partir de entradas analógicas
00009 PwmOut servo1(PTB0);    // entrada analógica servo 1 - listo
00010 PwmOut servo2(PTB1);    // entrada analógica servo 2 - listo
00011 PwmOut servo3(PTB2);    // sentrada analógica servo 3 - listo
00012 
00013 int STOP=0;                     // estado detenido inicializado en 0
00014 
00015 //servo1.period(0.025);
00016 //servo2.period(0.025);
00017 //servo3.period(0.025);
00018 //servo1.pulsewidth(0.00055);
00019 //servo2.pulsewidth(0.0022);
00020 //servo3.pulsewidth(0.0022);
00021 
00022 // Declaración de pines de los motores
00023 //DigitalOut motor1(PTC16);
00024 //DigitalOut motor2(PTC13);
00025 //DigitalOut enable(PTB3);
00026 
00027 int motor1;
00028 int motor2;
00029 
00030 // Declaración de pines para los botones
00031 DebouncedIn boton1(PTC11);
00032 DebouncedIn boton2b(PTC10);
00033 DebouncedIn boton2s(PTC6);
00034 DebouncedIn boton3(PTC5);
00035 DebouncedIn boton1i(PTC4);
00036 DebouncedIn boton2i(PTC3);
00037 DebouncedIn boton3i(PTC0);
00038 DebouncedIn boton_p(PTC7);
00039 
00040 // Declaración pines del sensor de ultrasonido
00041 SRF05 sensor(PTD3,PTD1);
00042 
00043 // Declaración pines digitales para los led's
00044 DigitalOut led1(PTE5);
00045 DigitalOut led2(PTE4);
00046 DigitalOut led3(PTE3);
00047 
00048 // Declaración pines de salida para el motor principal tipo PWM 
00049 // Entradas tipo PWM Análogas para inversión de giro
00050 PwmOut giro1(PTC1);    
00051 PwmOut giro2(PTC2);
00052 DigitalOut enable(PTB3);
00053 
00054 //giro1.period_ms(20);
00055 //giro2.period_ms(20);
00056     
00057 //A continuación se describe el número de botones a usar
00058 // los pisos del ascensor, el botón de paro
00059 // y las distancias entre los pisos
00060 int botones[4],pisos[3],paro;
00061 int bussy=0;                    
00062 float distancia;
00063 float dist3=8.0;
00064 float dist2=28.0;
00065 float dist1=47.0;
00066 
00067 // Pulsadores 
00068 void pulsadores(void const *args){
00069     
00070     while(1) {
00071         if (boton_p.read()==1){    // Lectura de boton de paro
00072             STOP=1;           
00073             paro=1; 
00074             }
00075         if (boton1.read()==1){
00076             botones[0]=1; //luego irá al piso 1
00077             }
00078         if (boton2b.read()==1){
00079             botones[1]=1; //luego irá al piso 1
00080             }
00081         if (boton2s.read()==1){
00082             botones[2]=1; //luego irá al piso 1
00083             }
00084         if (boton3.read()==1){
00085             botones[3]=1; //luego irá al piso 1
00086             }
00087         if (boton1i.read()==1){
00088             pisos[0]=1; //luego irá al piso 1
00089             }
00090         if (boton2i.read()==1){
00091             pisos[1]=1;  //luego irá al piso 2
00092             }
00093         if (boton3i.read()==1){
00094             pisos[2]=1; //luego irá al piso 3
00095             }
00096       // Thread::wait(1000.0*0.02);
00097     }
00098 }
00099 
00100 void sensar(void const *args){
00101     
00102     while(1) {
00103         
00104         distancia=sensor.read(); //sensar
00105         
00106         //Thread::wait(1000.0*0.02);
00107         
00108         distancia=floor(distancia); 
00109     }
00110 }
00111 void subir(void){ 
00112 
00113     if(motor2==0){
00114         giro1.period_ms(20);
00115         giro2.period_ms(20);
00116         enable=1;
00117         motor1=1;
00118         
00119         //giro1=1;
00120         //motor2=0;
00121         //giro2=0;
00122         
00123         giro1.pulsewidth_ms(15);
00124         giro2.pulsewidth_ms(0);
00125         }
00126 }
00127 void bajar(void){
00128     
00129     if(motor1==0){
00130         giro1.period_ms(20);
00131         giro2.period_ms(20);
00132         enable=1;
00133         motor1=0;
00134         
00135         //giro1=0;
00136         //motor2=1;
00137         //giro2=1;
00138         
00139         giro1.pulsewidth_ms(0);
00140         giro2.pulsewidth_ms(8);
00141         }
00142 }
00143 void stop(void){          
00144 
00145     giro1.period_ms(20);
00146     giro2.period_ms(20);
00147     enable=0;
00148     motor1=0;
00149     
00150     //giro1=0;
00151     //motor2=0;
00152     //giro2=0;
00153     
00154     enable=0;
00155     giro1.pulsewidth_ms(0);
00156     giro2.pulsewidth_ms(0);
00157 }
00158 
00159 void tercero(void){
00160     
00161     bussy=1;
00162     botones[3]=0;
00163     pisos[2]=0;
00164     servo1.period(0.025);   
00165     servo1.pulsewidth(0.0008);
00166     servo2.period(0.025);   
00167     servo2.pulsewidth(0.0055);  
00168     servo3.period(0.025);   
00169     servo3.pulsewidth(0.0055);   
00170 
00171     int flag1=0,flag2=0;
00172     subir();
00173     
00174     while(1){
00175         
00176         if (botones[2]==1 && distancia>=dist2){
00177             flag2=1;
00178             }    
00179         
00180         if (flag2==1 && distancia==dist2 && flag1==0){
00181             stop();
00182             servo2.pulsewidth(0.0008); 
00183             wait(5);
00184             servo2.pulsewidth(0.0055);    
00185             flag1=1; 
00186             subir();    
00187             botones[2]=0;
00188             }
00189         
00190         if (botones[1]==1){
00191             pisos[1]=1; // va luego para el piso 2
00192             }
00193 
00194         if (distancia<=dist3){
00195             stop();
00196             servo3.pulsewidth(0.0008); 
00197             wait(5);
00198             servo3.pulsewidth(0.0055);
00199             break;
00200             }
00201         }
00202         bussy=0;
00203 }
00204 
00205 void segundo(void){
00206     
00207     botones[1]=0; 
00208     botones[2]=0;
00209     pisos[1]=0; 
00210     bussy=1;
00211     servo1.period(0.025);   
00212     servo1.pulsewidth(0.0008);
00213     servo2.period(0.025);   
00214     servo2.pulsewidth(0.0055);  
00215     servo3.period(0.025);   
00216     servo3.pulsewidth(0.0055);   
00217 
00218     if (distancia>(dist2+2.0)){
00219         subir(); 
00220         }
00221     else{
00222         
00223         if (distancia<(dist2-2.0)){
00224             bajar();}
00225             }
00226          
00227         while(1){
00228             if (distancia==dist2){
00229                 stop();
00230                 servo2.pulsewidth(0.0008); 
00231                 wait(5);
00232                 servo2.pulsewidth(0.0055);    
00233                 break;
00234                 }
00235         }
00236     bussy=0;
00237 }
00238 
00239 void primero(void){
00240         
00241         botones[0]=0;
00242         pisos[0]=0;
00243         bussy=1;
00244         servo1.period(0.025);   
00245         servo1.pulsewidth(0.0008);
00246         servo2.period(0.025);   
00247         servo2.pulsewidth(0.0055);  
00248         servo3.period(0.025);   
00249         servo3.pulsewidth(0.0055);   
00250         int flag1=0,flag2=0;
00251         bajar();
00252         
00253         while(1){
00254             
00255             if (botones[1]==1 && distancia<=dist2){
00256                 flag2=1;
00257                 }
00258             if (flag2==1 && distancia==dist2 && flag1==0){
00259                 stop();
00260                 servo2.pulsewidth(0.0008);
00261                 wait(5);
00262                 servo2.pulsewidth(0.0055);      
00263                 flag1=1;  
00264                 bajar();  
00265                 botones[1]=0;
00266                 }
00267             if(botones[2]==1){
00268                 pisos[1]=1;
00269                 }
00270             if (distancia>=dist1){
00271                 stop();
00272                 servo1.pulsewidth(0.0022);
00273                 wait(5);
00274                 servo1.pulsewidth(0.0008);
00275                 break;
00276                 }
00277         }
00278         bussy=0;
00279 }
00280 
00281 void bombillos(void const *args){
00282    
00283     while(1){
00284         
00285         if (motor2==1){
00286             if (distancia>=dist3 && distancia<dist2){
00287                 led3=1;
00288                 led2=0;
00289                 led1=0;  
00290                 }
00291             if (distancia>=dist2 && distancia<dist1){
00292                 led3=0;
00293                 led2=1;
00294                 led1=0;     
00295                 }  
00296             if (distancia>=dist1){
00297                 led3=0;
00298                 led2=0;
00299                 led1=1;     
00300                 }
00301         }
00302         if (motor1==1 || (motor1==0 && motor2==0)){
00303             
00304             if (distancia<=dist1 && distancia>dist2){
00305                 led3=0;
00306                 led2=0;
00307                 led1=1;  
00308                 }
00309             if (distancia<=dist2 && distancia>dist3){
00310                 led3=0;
00311                 led2=1;
00312                 led1=0;     
00313                 }
00314             if (distancia<=dist3){
00315                 led3=1;
00316                 led2=0;
00317                 led1=0;     
00318                 }
00319         }
00320         
00321         //Thread::wait(1000.0*0.02);
00322         
00323     }
00324 }
00325 
00326 void parar(void){
00327 
00328     servo1.period(0.025);   
00329     servo1.pulsewidth(0.0008);
00330     servo2.period(0.025);   
00331     servo2.pulsewidth(0.0055);  
00332     servo3.period(0.025);   
00333     servo3.pulsewidth(0.0055); 
00334     bajar();
00335     
00336     while(1) {
00337         if (distancia>=dist1){
00338             stop();
00339             servo1.pulsewidth(0.0008);
00340             botones[0]=0;
00341             botones[1]=0;
00342             botones[2]=0;
00343             botones[3]=0;
00344             pisos[0]=0;
00345             pisos[1]=0;
00346             pisos[2]=0;
00347             STOP=0;
00348             paro=0;
00349             break;
00350            }
00351     }
00352 }
00353 
00354 int main() {
00355     
00356     giro1.period_ms(20);
00357     giro2.period_ms(20);
00358     giro1.pulsewidth_ms(0);
00359     giro2.pulsewidth_ms(0);
00360     
00361     enable=0;
00362     motor1=0;
00363     motor2=0;
00364     led1=0;
00365     led2=0;
00366     led3=0;
00367     
00368     //Thread thread2(pulsadores);
00369     //Thread thread4(sensar);
00370     //Thread thread5(bombillos);
00371     
00372     botones[0]=0;
00373     botones[1]=0;
00374     botones[2]=0;
00375     botones[3]=0;
00376     pisos[0]=0;
00377     pisos[1]=0;
00378     pisos[2]=0;
00379     
00380     while(1) {
00381         if (paro==1 ){
00382             botones[0]=0;
00383             botones[1]=0;
00384             botones[2]=0;
00385             botones[3]=0;
00386             pisos[0]=0;
00387             pisos[1]=0;
00388             pisos[2]=0;
00389             STOP=0;
00390             paro=0;
00391             primero();
00392             }
00393         if (STOP==0 && bussy==0){
00394             
00395             if (pisos[0]==1){
00396                 pisos[0]=0;
00397                 primero();
00398                 }
00399             if ( pisos[1]==1){
00400                 pisos[1]=0;
00401                 segundo();
00402                 }
00403             if (pisos[2]==1){
00404                 pisos[2]=0;
00405                 tercero();
00406                 }   
00407             if (botones[0]==0 && botones[1]==0 && botones[2]==0 && botones[3]==0){
00408                 }
00409             if ( botones[3]==1){
00410                 botones[3]=0; 
00411                 tercero();
00412                 }
00413             if (botones[1]==1 || botones[2]==1){
00414                 botones[1]=0; 
00415                 botones[2]=0; 
00416                 segundo();
00417                 }
00418             if (botones[0]==1 ){
00419                 botones[0]=0;    
00420                 primero();  
00421                 }
00422         }
00423         
00424         //Thread::wait(1000.0*0.2);
00425         //myled = 0;
00426         //Thread::wait(1000.0*0.2);     
00427     }
00428 }