establece la posicion de referencia del brazo robotico, manda la posicion de los encoders por bluetooth y permite el control remoto a traves de un dispositivo android.
main.cpp
- Committer:
- javierjsp
- Date:
- 2017-06-09
- Revision:
- 6:644b77b2111e
- Parent:
- 5:053c61912254
- Child:
- 7:cb59c58cab78
File content as of revision 6:644b77b2111e:
#include "mbed.h" #include "Motor.h" #include "QEI.h" // declaramos los encoders QEI enc1(PG_4, PG_6, NC, 480, QEI::X4_ENCODING); // (P0, P1, indice, PulsosPorRev, TipoDeEncoder) QEI enc2(PG_7, PG_5, NC, 480, QEI::X4_ENCODING); QEI enc3(PD_10, PG_8, NC, 480, QEI::X4_ENCODING); QEI enc4(PG_14, PF_11, NC, 480, QEI::X4_ENCODING); QEI enc5(PF_12, PF_15, NC, 480, QEI::X4_ENCODING); QEI enc6(PF_13, PF_3, NC, 240, QEI::X4_ENCODING); // declaramos los motores Motor m1(PA_5, PC_8, PC_6); // (pwm, derecho, inverso) Motor m2(PA_6, PC_5, PD_8); Motor m3(PA_7, PA_12, PA_11); Motor m4(PB_6, PB_12, PB_11); Motor m5(PC_7, PB_2, PB_1); Motor m6(PB_9, PB_15, PB_13); Motor cinta(PB_8, PC_4, PF_5); // declaramos las entradas de los micropulsadores DigitalIn mp1(PA_9, PullUp); // (pin, modo) DigitalIn mp2(PA_8, PullUp); DigitalIn mp3(PB_10, PullUp); DigitalIn mp4(PB_4, PullUp); DigitalIn mp5(PB_5, PullUp); DigitalIn sensor(PB_3, PullUp); // declaramos el bluetooth Serial bt(PA_2, PA_3); // (RX, TX) (puerto serial 2) // declaramos otras variables que utilizaremos Ticker ticker1, ticker2; // iniciamos dos tickers DigitalOut led1 (LED1); // declaramos el LED1 DigitalOut led2 (LED2); // declaramos el LED2 DigitalOut led3 (LED3); // declaramos el LED3 int pulsos[6]={0,0,0,0,0,0}; // iniciamos el array pulsos int posiciones[10][6]; // declaramos los arrays de posiciones for (int i=0; i<10; i++) for (int j=0; j<10; j++) posiciones[i][j]; char dato=0; // declaramos el prototipo de las funciones que utilizaremos void enviar_bt(); // prototipo de la funcion que ejecutara el ticker1 void encenderLEDs(); // prototipo de la funcion que ejecutara el ticker1 void sethome(); // prototipo de la funcion que nos calibrara la posicion hard home de referencia void posicion(); // prototipo de la funcion que nos llevara hasta la posicion especificada void mover(int,int,float); // prototipo de la funcion que movera cada motor void frenar(int); // prototipo de la funcion que frenara cada motor void pinza(int); // prototipo de la funcion que abrira (-1) o cerrara (1) la pinza void movCinta(); // prototipo de la funcion que avanzara la cinta hasta detectar un objeto int main() { m1.period(0.0001); // establece el periodo del PWM, equivalente a una frecuencia de 10KHz. m2.period(0.0001); m3.period(0.0001); m4.period(0.0001); m5.period(0.0001); m6.period(0.0001); bt.baud(9600); // velocidad de transmision del bluetooth a 9600 baudios 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 ticker2.attach(&encenderLEDs, 0.05); // asignamos la funcion "encenderLED1" al protocolo de interrupcion del "ticker2" y le damos un intervalo de 0.05s sethome(); // establecemos la posicion de referencia while(1) { if(bt.readable()>0) { dato = bt.getc(); switch(dato) { case 'A': mover(1,1,1); wait(0.2); frenar(1); break; case 'B': mover(1,-1,1); wait(0.2); frenar(1); break; case 'C': mover(2,1,1); wait(0.2); frenar(2); break; case 'D': mover(2,-1,1); wait(0.2); frenar(2); break; case 'E': mover(3,1,1); wait(0.2); frenar(3); break; case 'F': mover(3,-1,1); wait(0.2); frenar(3); break; case 'G': mover(4,1,1); wait(0.2); frenar(4); break; case 'H': mover(4,-1,1); wait(0.2); frenar(4); break; case 'I': mover(5,1,1); wait(0.2); frenar(5); break; case 'J': mover(5,-1,1); wait(0.2); frenar(5); break; case 'K': pinza(-1); break; case 'L': pinza(1); break; case 'M': movCinta(); break; } } } } void enviar_bt() // definimos la funcion que ejecutara el ticker { pulsos[1]=enc1.getPulses(); pulsos[2]=enc2.getPulses(); pulsos[3]=enc3.getPulses(); pulsos[4]=enc4.getPulses(); pulsos[5]=enc5.getPulses(); 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 } void encenderLEDs() { if (mp5==0) led1=1; // si se pulsa el mp1 se enciende el LED1 else led1=0; // si no se apaga if (mp4==0) led3=1; // si se pulsa el mp1 se enciende el LED1 else led3=0; // si no se apaga } void sethome() { led2=1; // posicionar base int enc=0; int dir=-1; // mover primero hacia delante while (mp1==1) // Mientras no se pulse el microswitch { enc=enc1.getPulses(); // leemos el contador del encoder mover(1,dir,0.7); // movemos el motor en la dirección inicial wait(0.02); // esperamos 0.01s if (enc==enc1.getPulses()) // Si el contador no se ha incrementado, hemos llegado al extremo dir=1; // se cambia la dirección del motor } frenar(1); // el motor ha llegado a la posicion adecuada y se frena // posicionar brazo dir=-1; // mover primero hacia delante while (dir==-1) // mientras la direccion sea hacia delante: { while (mp2==0) // Mientras se mantenga pulsado el boton mover(2,dir,0.7); // se seguira moviendo el motor en la direccion inicial dir=1; // se cambia a direccion while (mp2==1) // Mientras el MP esté sin pulsar: { enc=enc2.getPulses(); // leemos el valor de los encoders mover(2,dir,0.7); // movemos el motor wait(0.02); // esperamos 0.01s if(enc==enc2.getPulses()) // Si el contador no ha cambiado { if (dir==-1) dir=1; // cambiamos la dirección else if (dir==1) dir=-1; } } } frenar(2); // el motor ha llegado a la posicion adecuada y se frena // posicionar antebrazo dir=-1; // mover primero hacia delante while (dir==-1) // mientras la direccion sea hacia delante: { while (mp3==0) // Mientras se mantenga pulsado el boton mover(3,dir,0.7); // se seguira moviendo el motor en la direccion inicial dir=1; // se cambia a direccion while (mp3==1) // Mientras el MP esté sin pulsar: { enc=enc3.getPulses(); // leemos el valor de los encoders mover(3,dir,0.7); // movemos el motor wait(0.02); // esperamos 0.01s if(enc==enc3.getPulses()) // Si el contador no ha cambiado { if (dir==-1) dir=1; // cambiamos la dirección else if (dir==1) dir=-1; } } } frenar(3); // el motor ha llegado a la posicion adecuada y se frena // posicionamiento del giro de la muñeca dir=-1; // mover primero hacia delante while (dir==-1) // Mientras que la dirección del motor sea 1: { while (mp4==0) // Mientras el MP esté pulsado mover(4,dir,0.7); // movemos los motores 4 y 5 dir=1; while (mp4==1) // Mientras el MP esté sin pulsar: { enc=enc5.getPulses(); // leemos el valor del los encoders mover(4,dir,0.7); // movemos los motores wait(0.02); // esperamos 0.01s if(enc==enc5.getPulses()) // Si el contador no ha cambiado { if (dir==-1) dir=1; // cambiamos la dirección else if (dir==1) dir=-1; } } } frenar(4); // la muñeca ha llegado a la posicion adecuada y se frenan los motores // posicionamiento de la rotacion de la muñeca while (mp5==1) // Mientras no se pulse el MS { mover(5,1,0.7); // se mueven los motores 4 y 5 wait(0.02); // esperamos 0.01s } frenar(5); // frenamos los motores pinza(-1); // abrimos la pinza frenar(0); // frenamos todos los motoes enc1.reset(); // reseteamos los encoders enc2.reset(); enc3.reset(); enc4.reset(); enc5.reset(); enc6.reset(); led2=0; } void posicion(int pos) { } void mover(int m, int dir, float v) { if((dir==1 || dir==(-1))&&(v>0 && v<=1)) //comprueva que los parametros pasados estan dentro de los valores deseados { switch (m) { case 0: // en caso de que el valor sea 0 movemos todos los motores m1.speed(dir*v); m2.speed(dir*v); m3.speed(dir*v); m4.speed(dir*v); m5.speed(dir*v); break; case 1: // en caso de que el valor sea 1 movemos el motor 1 (base) m1.speed(dir*v); break; case 2: // en caso de que el valor sea 2 movemos el motor 2 (hombro) m2.speed(dir*v); break; case 3: // en caso de que el valor sea 3 movemos el motor 3 (codo) m3.speed(dir*v); break; 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) m4.speed((-1)*dir*v); m5.speed(dir*v); break; case 5: // en caso de que el valor sea 5 movemos los motores 4 y 5 en el mismo sentido (rotar muñeca) m4.speed(dir*v); m5.speed(dir*v); break; case 6: // en caso de que el valor sea 6 movemos el motor 6 (pinza) m6.speed(dir*v); break; } } else // si los parametros no son correctos encendemos el LED3 led3=1; } void pinza(int a) { int enc=0, encOld=0; mover(6,a,1); // movemos la pinza encOld=enc6.getPulses(); // leemos la posicion del encoder wait(0.1); // espera 0.1s enc=enc6.getPulses(); // leemos la posicion nueva del encoder while(enc != encOld) // mientras la posicion antigua sea distinta de la nueva seguimos leyendo { wait(0.1); // espera 0.1s encOld=enc; // guardamos la posicion antigua del encoder enc=enc6.getPulses(); // leemos la posicion nueva del encoder } frenar(6); // cuando la pinza ha llegado al tope frenamos la pinza } void frenar(int m) { switch(m) { case 0: // en caso de que el valor sea 0 frenamos todos los motores m1.brake(); m2.brake(); m3.brake(); m4.brake(); m5.brake(); m6.brake(); break; case 1: // en caso de que el valor sea 1 frenamos el motor 1 m1.brake(); break; case 2: // en caso de que el valor sea 2 frenamos el motor 2 m2.brake(); break; case 3: // en caso de que el valor sea 3 frenamos el motor 3 m3.brake(); break; case 4: // en caso de que el valor sea 4 frenamos los motores 4 y 5 m4.brake(); m5.brake(); break; case 5: // en caso de que el valor sea 5 frenamos los motores 4 y 5 m4.brake(); m5.brake(); break; case 6: // en caso de que el valor sea 6 frenamos el motor 6 m6.brake(); break; } } void movCinta() { while (sensor==1) cinta.speed(1); cinta.brake(); }