main.cpp

00001 #include "mbed.h"
00002 #include "hcsr04.h"
00003 
00004 
00005 
00006 Serial pc(USBTX, USBRX); // tx, rx
00007 Ticker tickerMideDistancia;
00008 unsigned distancia=20;
00009 
00010 HCSR04  usensor(D7,D8); //(PinName TrigPin,PinName EchoPin):
00011 DigitalIn finalDer(D2);
00012 DigitalIn finalIzq(D3);
00013 DigitalOut step(D13);
00014 DigitalOut dir(D12);
00015 DigitalOut enable(D11);
00016 
00017 int contador=0;
00018 Timer tiempo;
00019 
00020 enum estados {cerrada, abriendose, cerrandose, abierta};
00021 estados estado;
00022 
00023 
00024 void mideDistancia()
00025 {
00026     usensor.start();
00027 }
00028 
00029 void paso(int d)
00030 {
00031     dir=d;
00032     step=1;
00033     wait_us(100);   // Este es el cambio de paso
00034     step=0;
00035     wait_us(900);  //Este hay que cambiar
00036 }
00037 
00038 
00039 
00040 
00041 
00042 
00043 void estadoCerrada()
00044 {
00045    
00046     if( (finalIzq == 1) && (finalDer==1)) {
00047         enable=1;
00048         estado=abriendose;
00049        // pc.printf("Distance =%d\n",distancia);
00050         //pc.printf("Final2 pulsado\r\n");
00051         
00052        
00053         //distancia < 5
00054         tiempo.reset();
00055         
00056     }
00057 }
00058 
00059 void estadoAbriendose()
00060 {
00061 
00062    // pc.printf("Distance =%d\n",distancia);
00063     if(tiempo.read() > 2) {
00064         
00065         contador++;
00066         estado=cerrandose;
00067         enable=0;
00068         
00069         tiempo.reset();
00070              
00071     } else  {
00072         
00073         paso(1);
00074         
00075         //pc.printf("paso\n");
00076         
00077     }
00078 }
00079 
00080 void estadoCerrandose(){
00081     
00082      //pc.printf("Distance =%d\n",distancia);
00083      if((tiempo.read() > 2) && contador<3) {
00084         
00085         
00086         estado=abriendose;
00087         
00088     } 
00089         
00090     else if (contador>=3){
00091         
00092         paso(0);
00093         
00094         
00095         }
00096     else if ((contador>=3) && (finalIzq==1)){
00097     
00098     estado = cerrada;    
00099     
00100     }
00101     
00102     
00103     else {
00104         
00105         paso(0);
00106         
00107         }
00108 }
00109 
00110 void estadoAbierta()
00111 {
00112         
00113         enable=1;
00114         estado=cerrandose;
00115         //pc.printf("Final1 pulsado\r\n");
00116         
00117     
00118 }
00119 
00120 int main()
00121 {
00122     pc.baud(115200);
00123     tickerMideDistancia.attach(&mideDistancia, 0.5);
00124     
00125     tiempo.reset();
00126     tiempo.start();
00127     
00128     estado=cerrada;
00129     //pc.printf("Estado cerrada\n");
00130     wait(3.0);
00131     while(1) {
00132         distancia=usensor.get_dist_cm();
00133         pc.printf("%d\n",distancia);
00134         switch ( estado ) {
00135             case cerrada:
00136                 estadoCerrada();
00137                 break;
00138             case abriendose:
00139                 estadoAbriendose();
00140                 break;
00141             case abierta:
00142                 estadoAbierta();
00143                 break;
00144             case cerrandose:
00145                 estadoCerrandose();
00146                 break;
00147             default:
00148                 break;
00149         }
00150     }
00151 }