Javier Sanchez Peña
control de un brazo robotico SCORBOT ER-III a traves de bluetooth con un dispositivo android.
Diff: main.cpp
- Revision:
- 4:ceef4dee8284
- Parent:
- 3:0b3d5b1850aa
- Child:
- 5:ae0bdb3aad14
--- a/main.cpp Fri Jun 16 17:06:29 2017 +0000
+++ b/main.cpp Mon Sep 04 17:57:54 2017 +0000
@@ -25,10 +25,14 @@
DigitalIn mp3(PB_10, PullUp);
DigitalIn mp4(PB_4, PullUp);
DigitalIn mp5(PB_5, PullUp);
+
+// Declaramos los pines del sensor, la sonda y el calefactor
DigitalIn sensor(PB_3, PullUp);
+AnalogIn sonda(PF_10);
+PwmOut calef(PE_10);
// declaramos el bluetooth
-Serial bt(PA_2, PA_3); // (RX, TX) (puerto serial 2)
+Serial bt(PA_2, PA_3); // (TX, RX) (puerto serial 2)
// declaramos otras variables que utilizaremos
Ticker ticker1; // iniciamos un ticker
@@ -36,10 +40,13 @@
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]={{0,0,0,0,0,0},{0,250,50,1004,80,100},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}, // sustituye aqui los valores por los de los encoders y deja el primer valor de cada posicion a 0
- {0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}}; // declaramos los arrays de posiciones
+int posiciones[20][6]={{0,0,0,0,0,0},{0,-3200,0,0,0,0},{0,-3200,1820,1396,0,0},{0,0,1661,1117,0,0},{0,3000,2050,1200,0,0}, // declaramos los arrays de posiciones
+ {0,3000,0,0,0,0},{0,5200,0,0,0,0},{0,5200,2100,2000,0,0},{0,0,0,0,0,0},{0,-1988,-900,-400,396,-395}, // sustituye aqui los valores por los de los encoders
+ {0,-1700,737,819,-53,-835},{0,-713,1690,1616,180,-955},{0,240,953,-2014,-1540,-2247},{0,950,65,-3970,863,-2050},{0,2200,1040,240,-348,-836}, // y deja el primer valor de cada posicion a 0
+ {0,2150,1370,-2066,-200,-980},{0,3150,1370,-2066,1893,1116},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};
char dato=0;
float V=1;
+float t=0;
// declaramos el prototipo de las funciones que utilizaremos
void enviar_bt(); // prototipo de la funcion que ejecutara el ticker1
@@ -49,21 +56,26 @@
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
+void calefactor(); // prototipo de la funcion que controlara el calefactor
float map(float,float,float,float,float); // prototipo de la funcion que hace una redimension de los parametros
int main()
{
- m1.period(0.001); // establece el periodo del PWM, equivalente a una frecuencia de 1KHz.
+ m1.period(0.001); // establece el periodo del PWM de los motores, equivalente a una frecuencia de 1KHz.
m2.period(0.001);
m3.period(0.001);
m4.period(0.001);
m5.period(0.001);
m6.period(0.001);
+ calef.period(1); // establece el periodo del PWM
+ calef.write(0.05); // establece el ciclo de trabajo al 10%
+
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.2s
-
+ 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
+
+ wait(5);
sethome(); // establecemos la posicion de referencia
while(1)
@@ -148,16 +160,46 @@
posicion(4);
break;
case 'R': // si el caracter es R se mueve a la posicion 5
- posicion(5);
+ sethome();
break;
case 'S': // si el caracter es S se mueve a la posicion 6
- posicion(6);
+ posicion(1);
+ posicion(2);
+ pinza(1);
+ posicion(0);
+ posicion(3);
+ pinza(-1);
+ posicion(0);
break;
case 'T': // si el caracter es T se mueve a la posicion 7
+ posicion(1);
+ posicion(2);
+ pinza(1);
+ posicion(0);
+ posicion(5);
+ posicion(4);
+ calefactor();
+ posicion(5);
+ posicion(6);
posicion(7);
+ pinza(-1);
+ posicion(6);
+ posicion(0);
break;
case 'U': // si el caracter es U se mueve a la posicion 8
- posicion(8);
+ posicion(9);
+ posicion(10);
+ posicion(11);
+ posicion(12);
+ pinza(1);
+ posicion(13);
+ posicion(14);
+ pinza(-1);
+ posicion(15);
+ posicion(16);
+ pinza(1);
+ posicion(0);
+ pinza(-1);
break;
case 'V': // si el caracter es V se mueve a la posicion 9
posicion(9);
@@ -179,27 +221,29 @@
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 los valores del array "pulsos" por bluetooth
+ t= map(sonda.read(),0,1,-10,50);
+ 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 temperatura de la sonda:%.2f\n ", pulsos[1],pulsos[2],pulsos[3],pulsos[4],pulsos[5],t); // envia los valores del array "pulsos" por bluetooth
}
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,1); // movemos el motor en la dirección inicial
- wait(0.01); // esperamos 0.01s
+ mover(1,dir,1); // movemos el motor en la dirección inicial
+ wait(0.05); // esperamos 0.01s
if (enc==enc1.getPulses()) // Si el contador no se ha incrementado, hemos llegado al extremo
{
if (dir==-1)
dir=1; // cambiamos la dirección
else if (dir==1)
dir=-1;
- } // se cambia la dirección del motor
+ } // se cambia la dirección del motor
}
frenar(1); // el motor ha llegado a la posicion adecuada y se frena
@@ -208,13 +252,13 @@
while (dir==-1) // mientras la direccion sea hacia delante:
{
while (mp2==0) // Mientras se mantenga pulsado el boton
- mover(2,dir,1); // se seguira moviendo el motor en la direccion inicial
+ mover(2,dir,1); // 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,1); // movemos el motor
- wait(0.01); // esperamos 0.01s
+ mover(2,dir,1); // movemos el motor
+ wait(0.05); // esperamos 0.01s
if(enc==enc2.getPulses()) // Si el contador no ha cambiado
{
if (dir==-1)
@@ -227,16 +271,16 @@
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:
+ 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,1); // se seguira moviendo el motor en la direccion inicial
- dir=1; // se cambia a direccion
+ mover(3,dir,1); // 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,1); // movemos el motor
+ mover(3,dir,1); // movemos el motor
wait(0.01); // esperamos 0.01s
if(enc==enc3.getPulses()) // Si el contador no ha cambiado
{
@@ -250,16 +294,16 @@
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:
+ 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(14,dir,1); // movemos los motores 4 y 5
+ mover(14,dir,1); // 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(14,dir,1); // movemos los motores
+ mover(14,dir,1); // movemos los motores
wait(0.01); // esperamos 0.01s
if(enc==enc5.getPulses()) // Si el contador no ha cambiado
{
@@ -270,15 +314,18 @@
}
}
}
- frenar(14); // la muñeca ha llegado a la posicion adecuada y se frenan los motores
+ frenar(14); // 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(15,1,1); // se mueven los motores 4 y 5
+ mover(15,1,1); // se mueven los motores 4 y 5
wait(0.01); // esperamos 0.01s
}
- frenar(15); // frenamos los motores
+ frenar(15); // frenamos los motores
+ mover(15,1,1); // giramos la pinza para que quede perpendicular al eje
+ wait(0.25);
+ frenar(15);
pinza(-1); // abrimos la pinza
@@ -372,11 +419,11 @@
case 6: // en caso de que el valor sea 6 movemos el motor 6 (pinza)
m6.speed(dir*v);
break;
- case 14: // en caso de que el valor sea 14 movemos el motor 4 en un sentido y el 5 en el otro (girar muñeca)
+ case 14: // en caso de que el valor sea 14 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 15: // en caso de que el valor sea 15 movemos los motores 4 y 5 en el mismo sentido (rotar muñeca)
+ case 15: // en caso de que el valor sea 15 movemos los motores 4 y 5 en el mismo sentido (rotar muñeca)
m4.speed(dir*v);
m5.speed(dir*v);
}
@@ -431,11 +478,11 @@
case 6: // en caso de que el valor sea 6 frenamos el motor 6
m6.brake();
break;
- case 14: // en caso de que el valor sea 4 frenamos los motores 4 y 5
+ case 14: // en caso de que el valor sea 4 frenamos los motores 4 y 5
m4.brake();
m5.brake();
break;
- case 15: // en caso de que el valor sea 5 frenamos los motores 4 y 5
+ case 15: // en caso de que el valor sea 5 frenamos los motores 4 y 5
m4.brake();
m5.brake();
}
@@ -445,4 +492,12 @@
while (sensor==1) // mientras el sensor no se active:
cinta.speed(1); // avanza la cinta
cinta.brake(); // cuando se ectiva la cinta se para el sensor
-}
\ No newline at end of file
+}
+
+void calefactor()
+{
+ while (map(sonda.read(),0,1,-10,50) > 40)
+ calef.write(1);
+ calef.write(0);
+}
+