main.cpp

00001 #include "mbed.h"
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 DigitalIn sensor(PB_3, PullUp);
00029 
00030 // declaramos el bluetooth
00031 Serial bt(PA_2, PA_3);          // (RX, TX) (puerto serial 2)
00032 
00033 // declaramos otras variables que utilizaremos
00034 Ticker ticker1;                 // iniciamos tickers
00035 DigitalOut led1 (LED1);         // declaramos el LED1 
00036 DigitalOut led2 (LED2);         // declaramos el LED2 
00037 DigitalOut led3 (LED3);         // declaramos el LED3 
00038 int pulsos[6]={0,0,0,0,0,0};    // iniciamos el array pulsos
00039 int posiciones[10];             // declaramos el array de posiciones 
00040 char dato=0;
00041 
00042 // declaramos el prototipo de las funciones que utilizaremos
00043 void enviar_bt();               // prototipo de la funcion que ejecutara el ticker1
00044 void posicion();                // prototipo de la funcion que nos llevara hasta la posicion especificada
00045 void mover(int,int,float);      // prototipo de la funcion que movera cada motor
00046 void frenar(int);               // prototipo de la funcion que frenara cada motor
00047 void pinza(int);                // prototipo de la funcion que abrira (-1) o cerrara (1) la pinza
00048 void movCinta();                // prototipo de la funcion que avanzara la cinta hasta detectar un objeto
00049 
00050 int main()
00051 {
00052     m1.period(0.0001);      // establece el periodo del PWM, equivalente a una frecuencia de 10KHz.
00053     m2.period(0.0001);
00054     m3.period(0.0001);
00055     m4.period(0.0001);
00056     m5.period(0.0001);
00057     m6.period(0.0001);
00058     
00059     bt.baud(9600);              // velocidad de transmision del bluetooth a 9600 baudios
00060     
00061     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
00062         
00063     while(1)
00064     {
00065         if(bt.readable()>0)
00066         {
00067             dato = bt.getc();
00068             switch(dato)
00069             {   
00070                 case 'A':
00071                 mover(1,1,1);
00072                 wait(0.2);
00073                 frenar(1);
00074                 break;
00075                 case 'B':
00076                 mover(1,-1,1);
00077                 wait(0.2);
00078                 frenar(1);
00079                 break;
00080                 case 'C':
00081                 mover(2,1,1);
00082                 wait(0.2);
00083                 frenar(2);
00084                 break;
00085                 case 'D':
00086                 mover(2,-1,1);
00087                 wait(0.2);
00088                 frenar(2);
00089                 break;
00090                 case 'E':
00091                 mover(3,1,1);
00092                 wait(0.2);
00093                 frenar(3);
00094                 break;
00095                 case 'F':
00096                 mover(3,-1,1);
00097                 wait(0.2);
00098                 frenar(3);
00099                 break;
00100                 case 'G':
00101                 mover(4,1,1);
00102                 wait(0.2);
00103                 frenar(4);
00104                 break;
00105                 case 'H':
00106                 mover(4,-1,1);
00107                 wait(0.2);
00108                 frenar(4);
00109                 break;
00110                 case 'I':
00111                 mover(5,1,1);
00112                 wait(0.2);
00113                 frenar(5);
00114                 break;
00115                 case 'J':
00116                 mover(5,-1,1);
00117                 wait(0.2);
00118                 frenar(5);
00119                 break;
00120                 case 'K':
00121                 pinza(-1);
00122                 break;
00123                 case 'L':
00124                 pinza(1);
00125                 break;
00126                 case 'M':
00127                 movCinta();
00128                 break;
00129             }
00130         }
00131     }
00132 }
00133 
00134 void enviar_bt()            // definimos la funcion que ejecutara el ticker
00135 {
00136     pulsos[1]=enc1.getPulses();
00137     pulsos[2]=enc2.getPulses();
00138     pulsos[3]=enc3.getPulses();
00139     pulsos[4]=enc4.getPulses();
00140     pulsos[5]=enc5.getPulses();
00141     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", pulsos[1],pulsos[2],pulsos[3],pulsos[4],pulsos[5]);                // envia el valor de "pulsos" por bluetooth
00142 }
00143 
00144 void posicion(int pos)
00145 {
00146     
00147 }
00148 
00149 void mover(int m, int dir, float v)
00150 {
00151     if((dir==1 || dir==(-1))&&(v>0 && v<=1))   //comprueva que los parametros pasados estan dentro de los valores deseados
00152     {
00153         switch (m)
00154         {
00155             case 0:             // en caso de que el valor sea 0 movemos todos los motores
00156             m1.speed(dir*v);
00157             m2.speed(dir*v);
00158             m3.speed(dir*v);
00159             m4.speed(dir*v);
00160             m5.speed(dir*v);
00161             break;
00162             case 1:             // en caso de que el valor sea 1 movemos el motor 1 (base)
00163             m1.speed(dir*v);
00164             break;
00165             case 2:             // en caso de que el valor sea 2 movemos el motor 2 (hombro)
00166             m2.speed(dir*v);
00167             break;
00168             case 3:             // en caso de que el valor sea 3 movemos el motor 3 (codo)
00169             m3.speed(dir*v);
00170             break;
00171             case 4:             // en caso de que el valor sea 4 movemos el motor 4 en un sentido y el 5 en el otro (girar muñeca)
00172             m4.speed((-1)*dir*v);
00173             m5.speed(dir*v);
00174             break;
00175             case 5:             // en caso de que el valor sea 5 movemos los motores 4 y 5 en el mismo sentido (rotar muñeca)
00176             m4.speed(dir*v);
00177             m5.speed(dir*v);
00178             break;
00179             case 6:             // en caso de que el valor sea 6 movemos el motor 6 (pinza)
00180             m6.speed(dir*v);
00181             break;
00182         }
00183     }
00184     else        // si los parametros no son correctos encendemos el LED3
00185     led3=1;
00186 }
00187 
00188 void pinza(int a)
00189 {
00190     int enc=0, encOld=0;
00191     mover(6,a,1);             // movemos la pinza
00192     encOld=enc6.getPulses();    // leemos la posicion del encoder
00193     wait(0.1);                  // espera 0.1s
00194     enc=enc6.getPulses();       // leemos la posicion nueva del encoder
00195     while(enc != encOld)        // mientras la posicion antigua sea distinta de la nueva seguimos leyendo
00196     {
00197         wait(0.1);              // espera 0.1s
00198         encOld=enc;             // guardamos la posicion antigua del encoder
00199         enc=enc6.getPulses();   // leemos la posicion nueva del encoder
00200     }        
00201     frenar(6);                  // cuando la pinza ha llegado al tope frenamos la pinza
00202 } 
00203 
00204 void frenar(int m)
00205 {
00206     switch(m)
00207     {
00208         case 0:         // en caso de que el valor sea 0 frenamos todos los motores
00209         m1.brake();
00210         m2.brake();
00211         m3.brake();
00212         m4.brake();
00213         m5.brake();
00214         m6.brake();
00215         break;
00216         case 1:         // en caso de que el valor sea 1 frenamos el motor 1
00217         m1.brake();
00218         break;
00219         case 2:         // en caso de que el valor sea 2 frenamos el motor 2
00220         m2.brake();
00221         break;
00222         case 3:         // en caso de que el valor sea 3 frenamos el motor 3
00223         m3.brake();
00224         break;
00225         case 4:         // en caso de que el valor sea 4 frenamos los motores 4 y 5
00226         m4.brake();
00227         m5.brake();
00228         break;
00229         case 5:         // en caso de que el valor sea 5 frenamos los motores 4 y 5
00230         m4.brake();
00231         m5.brake();
00232         break;
00233         case 6:         // en caso de que el valor sea 6 frenamos el motor 6
00234         m6.brake();
00235         break;
00236     }
00237 }
00238 void movCinta()
00239 {
00240     while (sensor==1)
00241         cinta.speed(1);
00242     cinta.brake();
00243 }
00244 
00245 
00246