Javier Sanchez Peña
/
bluetooth_brazo_robotico
control del brazo robotico por bluetooth mediante un dispositivo android
main.cpp
- Committer:
- javierjsp
- Date:
- 2017-06-08
- Revision:
- 0:8110163d3863
- Child:
- 1:cc623c325046
File content as of revision 0:8110163d3863:
#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; // iniciamos 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]; // declaramos el array de posiciones char dato=0; // declaramos el prototipo de las funciones que utilizaremos void enviar_bt(); // prototipo de la funcion que ejecutara el ticker1 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 int main() { m1.period(0.001); // establece el periodo del PWM, equivalente a una frecuencia de 10KHz. m2.period(0.001); m3.period(0.001); m4.period(0.001); m5.period(0.001); m6.period(0.001); 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 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; } } } } 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 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; } }