main.cpp

00001 #include "mbed.h"           // declaramos las librerias de funciones 
00002 #include "Motor.h"
00003 #include "QEI.h"
00004 
00005 // declaramos los encoders
00006 QEI enc1(PG_4, PG_6, NC, 480, QEI::X4_ENCODING);      // (P0, P1, indice, PulsosPorRev, TipoDeEncoder)
00007 QEI enc2(PG_7, PG_5, NC, 480, QEI::X4_ENCODING);
00008 QEI enc3(PD_10, PG_8, NC, 480, QEI::X4_ENCODING);
00009 QEI enc4(PG_14, PF_11, NC, 480, QEI::X4_ENCODING);
00010 QEI enc5(PF_12, PF_15, NC, 480, QEI::X4_ENCODING);
00011 QEI enc6(PF_13, PF_3, NC, 240, QEI::X4_ENCODING);
00012 
00013 // declaramos los motores
00014 Motor m1(PA_5, PC_8, PC_6);      // (pwm, derecho, inverso)
00015 Motor m2(PA_6, PC_5, PD_8);
00016 Motor m3(PA_7, PA_12, PA_11);
00017 Motor m4(PB_6, PB_12, PB_11);
00018 Motor m5(PC_7, PB_2, PB_1);
00019 Motor m6(PB_9, PB_15, PB_13);
00020 Motor cinta(PB_8, PC_4, PF_5);
00021 
00022 // declaramos las entradas de los micropulsadores
00023 DigitalIn mp1(PA_9, PullUp);      // (pin, modo)
00024 DigitalIn mp2(PA_8, PullUp);
00025 DigitalIn mp3(PB_10, PullUp);
00026 DigitalIn mp4(PB_4, PullUp);
00027 DigitalIn mp5(PB_5, PullUp);
00028 
00029 // Declaramos los pines del sensor, la sonda y el calefactor
00030 DigitalIn sensor(PB_3, PullUp);
00031 AnalogIn sonda(PF_10);
00032 PwmOut calef(PE_10);
00033 
00034 // declaramos el bluetooth
00035 Serial bt(PA_2, PA_3);          // (TX, RX) (puerto serial 2)
00036 
00037 // declaramos otras variables que utilizaremos
00038 Ticker ticker1;                 // iniciamos un ticker
00039 DigitalOut led1 (LED1);         // declaramos el LED1 
00040 DigitalOut led2 (LED2);         // declaramos el LED2 
00041 DigitalOut led3 (LED3);         // declaramos el LED3 
00042 int pulsos[6]={0,0,0,0,0,0};    // iniciamos el array pulsos
00043 int posiciones[20][6]={{0,0,0,0,0,0},{0,-3200,0,0,0,0},{0,-3200,1820,1396,0,0},{0,0,1661,1117,0,0},{0,3000,2050,1200,0,0},  // declaramos los arrays de posiciones 
00044                        {0,3000,0,0,0,0},{0,5200,0,0,0,0},{0,5200,2100,2000,0,0},{0,0,0,0,0,0},{0,-1988,-900,-400,396,-395},           // sustituye aqui los valores por los de los encoders 
00045                        {0,-1700,737,819,-53,-835},{0,-713,1690,1616,180,-955},{0,240,953,-2014,-1540,-2247},{0,950,65,-3970,863,-2050},{0,2200,1040,240,-348,-836},           // y deja el primer valor de cada posicion a 0
00046                        {0,2150,1370,-2066,-200,-980},{0,3150,1370,-2066,1893,1116},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};          
00047 char dato=0;
00048 float V=1;
00049 float t=0;
00050 
00051 // declaramos el prototipo de las funciones que utilizaremos
00052 void enviar_bt();               // prototipo de la funcion que ejecutara el ticker1
00053 void sethome();                 // prototipo de la funcion que nos calibrara la posicion hard home de referencia
00054 void posicion(int);             // prototipo de la funcion que nos llevara hasta la posicion especificada
00055 void mover(int,int,float);      // prototipo de la funcion que movera cada motor
00056 void frenar(int);               // prototipo de la funcion que frenara cada motor
00057 void pinza(int);                // prototipo de la funcion que abrira (-1) o cerrara (1) la pinza
00058 void movCinta();                // prototipo de la funcion que avanzara la cinta hasta detectar un objeto
00059 void calefactor();              // prototipo de la funcion que controlara el calefactor
00060 float map(float,float,float,float,float);   // prototipo de la funcion que hace una redimension de los parametros
00061 
00062 int main()
00063 {
00064     m1.period(0.001);       // establece el periodo del PWM de los motores, equivalente a una frecuencia de 1KHz.
00065     m2.period(0.001);
00066     m3.period(0.001);
00067     m4.period(0.001);
00068     m5.period(0.001);
00069     m6.period(0.001);
00070     
00071     calef.period(1);        // establece el periodo del PWM 
00072     calef.write(0.05);       // establece el ciclo de trabajo al 10%
00073     
00074     bt.baud(9600);          // velocidad de transmision del bluetooth a 9600 baudios
00075     
00076     ticker1.attach(&enviar_bt, 0.5);    // asignamos la funcion "enviar_bt" al protocolo de interrupcion del "ticker1" y le damos un intervalo de 0.5s
00077     
00078     wait(5);
00079     sethome();              // establecemos la posicion de referencia
00080     
00081     while(1)
00082     {
00083         if(bt.readable()>0)             // si hay un dato disponible:
00084         {
00085             dato = bt.getc();           // leemos un caracter
00086             if (dato<=10 && dato>=1)    // si tiene un valor entre 1 y 10 lo asignamos como velocidad
00087                 V=map(dato,1,10,0.6,1);
00088             switch(dato)
00089             {   
00090                 case 'A':       // si el caracter es A se mueve el motor 1 al derecho
00091                 mover(1,1,V);
00092                 wait(0.2);
00093                 frenar(1);
00094                 break;
00095                 case 'B':       // si el caracter es B se mueve el motor 1 al inverso
00096                 mover(1,-1,V);
00097                 wait(0.2);
00098                 frenar(1);
00099                 break;
00100                 case 'C':       // si el caracter es C se mueve el motor 2 al derecho
00101                 mover(2,1,V);
00102                 wait(0.2);
00103                 frenar(2);
00104                 break;
00105                 case 'D':       // si el caracter es D se mueve el motor 2 al inverso
00106                 mover(2,-1,V);
00107                 wait(0.2);
00108                 frenar(2);
00109                 break;
00110                 case 'E':       // si el caracter es E se mueve el motor 3 al derecho
00111                 mover(3,1,V);
00112                 wait(0.2);
00113                 frenar(3);
00114                 break;
00115                 case 'F':       // si el caracter es F se mueve el motor 3 al inverso
00116                 mover(3,-1,V);
00117                 wait(0.2);
00118                 frenar(3);
00119                 break;
00120                 case 'G':       // si el caracter es G se mueve el motor 4 Y 5 al derecho
00121                 mover(14,1,V);
00122                 wait(0.2);
00123                 frenar(14);
00124                 break;
00125                 case 'H':       // si el caracter es H se mueve el motor 4 Y 5 al inverso
00126                 mover(14,-1,V);
00127                 wait(0.2);
00128                 frenar(14);
00129                 break;
00130                 case 'I':       // si el caracter es I se mueve el motor 4 Y 5 al derecho
00131                 mover(15,1,V);
00132                 wait(0.2);
00133                 frenar(15);
00134                 break;
00135                 case 'J':       // si el caracter es J se mueve el motor 4 Y 5 al inverso
00136                 mover(15,-1,V);
00137                 wait(0.2);
00138                 frenar(15);
00139                 break;
00140                 case 'K':       // si el caracter es K se abre la pinza
00141                 pinza(-1);
00142                 break;
00143                 case 'L':       // si el caracter es L se cierra la pinza
00144                 pinza(1);
00145                 break;
00146                 case 'M':       // si el caracter es M se mueve a la posicion home
00147                 posicion(0);
00148                 pinza(-1);
00149                 break;
00150                 case 'N':       // si el caracter es N se mueve a la posicion 1
00151                 posicion(1);
00152                 break;
00153                 case 'O':       // si el caracter es O se mueve a la posicion 2
00154                 posicion(2);
00155                 break;
00156                 case 'P':       // si el caracter es P se mueve a la posicion 3
00157                 posicion(3);
00158                 break;
00159                 case 'Q':       // si el caracter es Q se mueve a la posicion 4
00160                 posicion(4);
00161                 break;
00162                 case 'R':       // si el caracter es R se mueve a la posicion 5
00163                 sethome();
00164                 break;
00165                 case 'S':       // si el caracter es S se mueve a la posicion 6
00166                 posicion(1);
00167                 posicion(2);
00168                 pinza(1);
00169                 posicion(0);
00170                 posicion(3);
00171                 pinza(-1);
00172                 posicion(0);
00173                 break;
00174                 case 'T':       // si el caracter es T se mueve a la posicion 7
00175                 posicion(1);
00176                 posicion(2);
00177                 pinza(1);
00178                 posicion(0);
00179                 posicion(5);
00180                 posicion(4);
00181                 calefactor();
00182                 posicion(5);
00183                 posicion(6);
00184                 posicion(7);
00185                 pinza(-1);
00186                 posicion(6);
00187                 posicion(0);
00188                 break;
00189                 case 'U':       // si el caracter es U se mueve a la posicion 8
00190                 posicion(9);
00191                 posicion(10);
00192                 posicion(11);
00193                 posicion(12);
00194                 pinza(1);
00195                 posicion(13);
00196                 posicion(14);
00197                 pinza(-1);
00198                 posicion(15);
00199                 posicion(16);
00200                 pinza(1);
00201                 posicion(0);
00202                 pinza(-1);                
00203                 break;
00204                 case 'V':       // si el caracter es V se mueve a la posicion 9
00205                 posicion(9);
00206                 break;
00207             }
00208         }
00209     }
00210 }
00211 
00212 float map(float x, float in_min, float in_max, float out_min, float out_max)    // funcion que redimensiona los parametros
00213 {
00214   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00215 }
00216 
00217 void enviar_bt()            // definimos la funcion que ejecutara el ticker
00218 {
00219     pulsos[1]=enc1.getPulses();     // cargamos los contadores de los encoders
00220     pulsos[2]=enc2.getPulses();
00221     pulsos[3]=enc3.getPulses();
00222     pulsos[4]=enc4.getPulses();
00223     pulsos[5]=enc5.getPulses();
00224     t= map(sonda.read(),0,1,-10,50);
00225     bt.printf(" pulsos encoder 1:%d\n pulsos encoder 2:%d\n pulsos encoder 3:%d\n pulsos encoder 4:%d\n pulsos encoder 5:%d\n temperatura de la sonda:%.2f\n ", pulsos[1],pulsos[2],pulsos[3],pulsos[4],pulsos[5],t); // envia los valores del array "pulsos" por bluetooth
00226 }
00227 
00228 void sethome()
00229 {
00230     led2=1;
00231     
00232     // posicionar base
00233     int enc=0;
00234     int dir=-1;                 // mover primero hacia delante
00235     while (mp1==1)              // Mientras no se pulse el microswitch
00236     {
00237         enc=enc1.getPulses();   // leemos el contador del encoder
00238         mover(1,dir,1);         // movemos el motor en la dirección inicial
00239         wait(0.05);             // esperamos 0.01s
00240         if (enc==enc1.getPulses())  // Si el contador no se ha incrementado, hemos llegado al extremo
00241             {
00242                 if (dir==-1)
00243                 dir=1;          // cambiamos la dirección
00244                 else if (dir==1)
00245                 dir=-1;
00246             }                   // se cambia la dirección del motor
00247     }
00248     frenar(1);                  // el motor ha llegado a la posicion adecuada y se frena
00249     
00250     // posicionar brazo
00251     dir=-1;                     // mover primero hacia delante
00252     while (dir==-1)             // mientras la direccion sea hacia delante:
00253     {
00254         while (mp2==0)          // Mientras se mantenga pulsado el boton
00255             mover(2,dir,1);     // se seguira moviendo el motor en la direccion inicial
00256         dir=1;                  // se cambia a direccion
00257         while (mp2==1)          // Mientras el MP esté sin pulsar:
00258         {
00259             enc=enc2.getPulses();   // leemos el valor de los encoders
00260             mover(2,dir,1);     // movemos el motor
00261             wait(0.05);         // esperamos 0.01s
00262             if(enc==enc2.getPulses())   // Si el contador no ha cambiado
00263             {
00264                 if (dir==-1)
00265                 dir=1;          // cambiamos la dirección
00266                 else if (dir==1)
00267                 dir=-1;
00268             }
00269         }
00270     }
00271     frenar(2);                  // el motor ha llegado a la posicion adecuada y se frena
00272     
00273     // posicionar antebrazo
00274     dir=-1;                     // mover primero hacia delante
00275     while (dir==-1)             // mientras la direccion sea hacia delante:
00276     {
00277         while (mp3==0)          // Mientras se mantenga pulsado el boton
00278             mover(3,dir,1);     // se seguira moviendo el motor en la direccion inicial
00279         dir=1;                  // se cambia a direccion
00280         while (mp3==1)          // Mientras el MP esté sin pulsar:
00281         {
00282             enc=enc3.getPulses();   // leemos el valor de los encoders
00283             mover(3,dir,1);     // movemos el motor
00284             wait(0.01);         // esperamos 0.01s
00285             if(enc==enc3.getPulses())   // Si el contador no ha cambiado
00286             {
00287                 if (dir==-1)
00288                 dir=1;          // cambiamos la dirección
00289                 else if (dir==1)
00290                 dir=-1;
00291             }
00292         }
00293     }
00294     frenar(3);                  // el motor ha llegado a la posicion adecuada y se frena
00295     
00296     // posicionamiento del giro de la muñeca
00297     dir=-1;                     // mover primero hacia delante
00298     while (dir==-1)             // Mientras que la dirección del motor sea 1:
00299     {
00300         while (mp4==0)          // Mientras el MP esté pulsado
00301             mover(14,dir,1);    // movemos los motores 4 y 5
00302         dir=1;
00303         while (mp4==1)          // Mientras el MP esté sin pulsar:
00304         {
00305             enc=enc5.getPulses();   // leemos el valor del los encoders
00306             mover(14,dir,1);    // movemos los motores
00307             wait(0.01);         // esperamos 0.01s
00308             if(enc==enc5.getPulses())   // Si el contador no ha cambiado
00309             {
00310                 if (dir==-1)
00311                 dir=1;          // cambiamos la dirección
00312                 else if (dir==1)
00313                 dir=-1;
00314             }
00315         }
00316     }
00317     frenar(14);                 // la muñeca ha llegado a la posicion adecuada y se frenan los motores
00318     
00319     // posicionamiento de la rotacion de la muñeca
00320     while (mp5==1)              // Mientras no se pulse el MS
00321     {
00322         mover(15,1,1);          // se mueven los motores 4 y 5 
00323         wait(0.01);             // esperamos 0.01s
00324     }
00325     frenar(15);                 // frenamos los motores
00326     mover(15,1,1);              // giramos la pinza para que quede perpendicular al eje
00327     wait(0.25);
00328     frenar(15);
00329     
00330     pinza(-1);                  // abrimos la pinza
00331     
00332     // ponemos el contador de los encoders a 0
00333     frenar(0);                  // frenamos todos los motoes
00334     enc1.reset();               // reseteamos los encoders
00335     enc2.reset();
00336     enc3.reset();
00337     enc4.reset();
00338     enc5.reset();
00339     enc6.reset();
00340     led2=0;
00341 }
00342 
00343 void posicion(int pos)
00344 {
00345     frenar(0);                      // nos aseguramos de que los motores estan parados
00346     pulsos[1]=enc1.getPulses();     // leemos los valores de los conadores de los encoders
00347     pulsos[2]=enc2.getPulses();
00348     pulsos[3]=enc3.getPulses();
00349     pulsos[4]=enc4.getPulses();
00350     pulsos[5]=enc5.getPulses();
00351     int dir[6]={0,0,0,0,0,0};       // declaramos un array para las direcciones
00352     int flag[6]={1,1,1,1,1,1};      // declaramos un array para banderas
00353     int i;
00354     for (i=1;i<6;i++)               // establecemos la direccion a la que tiene que ir cada motor 
00355     {
00356         if (posiciones[pos][i]<pulsos[i])
00357             dir[i]=-1;
00358         else if (posiciones[pos][i]>pulsos[i])
00359             dir[i]=1;
00360         else if (posiciones[pos][i]==pulsos[i])
00361             flag[i]=0;
00362     }
00363     while (flag[1]+flag[2]+flag[3]+flag[4]+flag[5]) // mientras que alguna de las banderas este activada:
00364     {
00365         for (i=1;i<6;i++)       // repetimos para cada uno de los motores
00366         {
00367             if (flag[i]==1)     // si la bandera del motor i esta activa
00368             {
00369                 mover(i,dir[i],V);  // movemos el motor en la direccion establecida 
00370                 if (dir[i]==1 && pulsos[i]>=posiciones[pos][i]) // si ha llegado a la posicion deseada
00371                 {
00372                     frenar(i);      // frenamos el motor i
00373                     flag[i]=0;      // desactivamos su bandera
00374                 }
00375                 if (dir[i]==-1 && pulsos[i]<=posiciones[pos][i]) // si ha llegado a la posicion deseada
00376                 {
00377                     frenar(i);      // frenamos el motor i
00378                     flag[i]=0;      // desactivamos su bandera
00379                 }
00380             }
00381         }
00382         pulsos[1]=enc1.getPulses();     // leemos la nueva posicion de los encoders
00383         pulsos[2]=enc2.getPulses();
00384         pulsos[3]=enc3.getPulses();
00385         pulsos[4]=enc4.getPulses();
00386         pulsos[5]=enc5.getPulses();
00387     }
00388 wait(0.1);              // esperamos 0.1s
00389 }
00390 
00391 void mover(int m, int dir, float v)
00392 {
00393     if((dir==1 || dir==(-1))&&(v>0 && v<=1))   //comprueva que los parametros pasados estan dentro de los valores deseados
00394     {
00395         switch (m)
00396         {
00397             case 0:             // en caso de que el valor sea 0 movemos todos los motores
00398             m1.speed(dir*v);
00399             m2.speed(dir*v);
00400             m3.speed(dir*v);
00401             m4.speed(dir*v);
00402             m5.speed(dir*v);
00403             break;
00404             case 1:             // en caso de que el valor sea 1 movemos el motor 1 (base)
00405             m1.speed(dir*v);
00406             break;
00407             case 2:             // en caso de que el valor sea 2 movemos el motor 2 (hombro)
00408             m2.speed(dir*v);
00409             break;
00410             case 3:             // en caso de que el valor sea 3 movemos el motor 3 (codo)
00411             m3.speed(dir*v);
00412             break;
00413             case 4:             // en caso de que el valor sea 4 movemos el motor 4 
00414             m4.speed(dir*v);
00415             break;
00416             case 5:             // en caso de que el valor sea 5 movemos el motor 5
00417             m5.speed(dir*v);
00418             break;
00419             case 6:             // en caso de que el valor sea 6 movemos el motor 6 (pinza)
00420             m6.speed(dir*v);
00421             break;
00422             case 14:            // en caso de que el valor sea 14 movemos el motor 4 en un sentido y el 5 en el otro (girar muñeca)
00423             m4.speed((-1)*dir*v);
00424             m5.speed(dir*v);
00425             break;
00426             case 15:            // en caso de que el valor sea 15 movemos los motores 4 y 5 en el mismo sentido (rotar muñeca)
00427             m4.speed(dir*v);
00428             m5.speed(dir*v);
00429         }
00430     }
00431     else        // si los parametros no son correctos encendemos el LED3
00432     led3=1;
00433 }
00434 
00435 void pinza(int a)
00436 {
00437     int enc=0, encOld=0;
00438     mover(6,a,1);               // movemos la pinza
00439     encOld=enc6.getPulses();    // leemos la posicion del encoder
00440     wait(0.1);                  // espera 0.1s
00441     enc=enc6.getPulses();       // leemos la posicion nueva del encoder
00442     while(enc != encOld)        // mientras la posicion antigua sea distinta de la nueva seguimos leyendo
00443     {
00444         wait(0.1);              // espera 0.1s
00445         encOld=enc;             // guardamos la posicion antigua del encoder
00446         enc=enc6.getPulses();   // leemos la posicion nueva del encoder
00447     }        
00448     frenar(6);                  // cuando la pinza ha llegado al tope frenamos la pinza
00449 } 
00450 
00451 void frenar(int m)
00452 {
00453     switch(m)
00454     {
00455         case 0:         // en caso de que el valor sea 0 frenamos todos los motores
00456         m1.brake();
00457         m2.brake();
00458         m3.brake();
00459         m4.brake();
00460         m5.brake();
00461         m6.brake();
00462         break;
00463         case 1:         // en caso de que el valor sea 1 frenamos el motor 1
00464         m1.brake();
00465         break;
00466         case 2:         // en caso de que el valor sea 2 frenamos el motor 2
00467         m2.brake();
00468         break;
00469         case 3:         // en caso de que el valor sea 3 frenamos el motor 3
00470         m3.brake();
00471         break;
00472         case 4:         // en caso de que el valor sea 4 frenamos el motor 4 
00473         m4.brake();
00474         break;
00475         case 5:         // en caso de que el valor sea 5 frenamos el motor 5
00476         m5.brake();
00477         break;
00478         case 6:         // en caso de que el valor sea 6 frenamos el motor 6
00479         m6.brake();
00480         break;
00481         case 14:        // en caso de que el valor sea 4 frenamos los motores 4 y 5
00482         m4.brake();
00483         m5.brake();
00484         break;
00485         case 15:        // en caso de que el valor sea 5 frenamos los motores 4 y 5
00486         m4.brake();
00487         m5.brake();
00488     }
00489 }
00490 void movCinta()
00491 {
00492     while (sensor==1)   // mientras el sensor no se active:
00493         cinta.speed(1); // avanza la cinta
00494     cinta.brake();      // cuando se ectiva la cinta se para el sensor
00495 }
00496 
00497 void calefactor()
00498 {
00499     while (map(sonda.read(),0,1,-10,50) < 40)       // mientras la temperatura sea menor que 40º activa el calefactor
00500     calef.write(1);
00501     calef.write(0);
00502 }
00503