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.
Diff: main.cpp
- Revision:
- 4:9ea8a9271732
- Parent:
- 3:fa1d943cf383
- Child:
- 5:053c61912254
--- a/main.cpp Mon Jun 05 23:46:15 2017 +0000 +++ b/main.cpp Tue Jun 06 23:48:39 2017 +0000 @@ -31,7 +31,7 @@ Serial bt(PA_2, PA_3); // (RX, TX) (puerto serial 2) // declaramos otras variables que utilizaremos -Ticker ticker1, ticker2; // iniciamos dos tickers +Ticker ticker1, ticker2, ticker3; // iniciamos dos tickers DigitalOut led1 (LED1); // declaramos el LED1 DigitalOut led2 (LED2); // declaramos el LED2 DigitalOut led3 (LED3); // declaramos el LED3 @@ -42,6 +42,7 @@ // declaramos el prototipo de las funciones que utilizaremos void enviar_bt(); // prototipo de la funcion que ejecutara el ticker1 void encenderLED1(); // prototipo de la funcion que ejecutara el ticker1 +void encenderLED3(); // 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 @@ -50,18 +51,19 @@ 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); + 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 ticker2.attach(&encenderLED1, 0.05); // asignamos la funcion "encenderLED1" al protocolo de interrupcion del "ticker2" y le damos un intervalo de 0.05s - + ticker3.attach(&encenderLED3, 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) @@ -134,15 +136,26 @@ 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 encenderLED1() { - if (mp1==0) led1=1; // si se pulsa el mp1 se enciende el LED1 + if (mp5==0) led1=1; // si se pulsa el mp1 se enciende el LED1 else led1=0; // si no se apaga } +void encenderLED3() +{ + 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; @@ -152,7 +165,7 @@ while (mp1==1) // Mientras no se pulse el microswitch { enc=enc1.getPulses(); // leemos el contador del encoder - mover(1,dir,0.4); // movemos el motor en la dirección inicial + mover(1,dir,0.7); // movemos el motor en la dirección inicial wait(0.01); // 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 @@ -164,15 +177,22 @@ while (dir==1) // mientras la direccion sea hacia delante: { while (mp2==0) // Mientras se mantenga pulsado el boton - mover(2,dir,0.4); // se seguira moviendo el motor en la direccion inicial + { + 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.4); // movemos el motor + mover(2,dir,0.7); // movemos el motor wait(0.01); // 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 @@ -182,15 +202,20 @@ while (dir==1) // mientras la direccion sea hacia delante: { while (mp3==0) // Mientras se mantenga pulsado el boton - mover(3,dir,0.4); // se seguira moviendo el motor en la direccion inicial + 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.4); // movemos el motor + mover(3,dir,0.7); // movemos el motor wait(0.01); // 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 @@ -200,23 +225,28 @@ while (dir==1) // Mientras que la dirección del motor sea 1: { while (mp4==0) // Mientras el MP esté pulsado - mover(4,dir,0.4); // movemos los motores 4 y 5 + 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.4); // movemos los motores + mover(4,dir,0.7); // movemos los motores wait(0.01); // 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) // Mientras no se pulse el MS + while (mp5==1) // Mientras no se pulse el MS { - mover(5,1,0.4); // se mueven los motores 4 y 5 + mover(5,1,0.7); // se mueven los motores 4 y 5 wait(0.01); // esperamos 0.01s } frenar(5); // frenamos los motores