Javier Sanchez Peña
/
bluetooth_brazo_robotico
control del brazo robotico por bluetooth mediante un dispositivo android
Diff: main.cpp
- Revision:
- 0:8110163d3863
- Child:
- 1:cc623c325046
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Jun 08 20:59:13 2017 +0000 @@ -0,0 +1,235 @@ +#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; + } +} + +