Diff: main.cpp

Revision:

0:b7978deb977b

Child:

1:7c07ddc42a4c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 30 02:13:36 2017 +0000
@@ -0,0 +1,316 @@
+#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);
+
+// 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 mp6(PB_3, PullUp);
+
+// declaramos el bluetooth
+Serial bt(PA_2, PA_3);          // (RX, TX) (puerto serial 2)
+
+Ticker ticker1;                 // iniciamos un ticker
+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
+char dato=0;
+
+// declaramos el prototipo de las funciones que utilizaremos
+void enviar_bt();               // prototipo de la funcion que ejecutara el ticker
+void sethome();                 // prototipo de la funcion que nos calibrara la posicion hard home de referencia
+void home();                    // prototipo de la funcion que nos llevara hasta la posicion hard home
+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.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" a la interrupcion del "ticker1" y le damos un intervalo de 0.5s
+    
+    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;
+            }
+        }
+    }
+}
+
+void enviar_bt()            // definimos la funcion que ejecutara el ticker
+{
+    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 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
+        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,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
+            if(enc==enc2.getPulses())   // Si el contador no ha cambiado
+                dir=1;          // cambiamos la dirección
+        }
+    }
+    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,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
+            wait(0.01);         // esperamos 0.01s
+            if(enc==enc3.getPulses())   // Si el contador no ha cambiado
+                dir=1;          // cambiamos la dirección
+        }
+    }
+    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,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(4,dir,1);     // movemos los motores
+            wait(0.01);         // esperamos 0.01s
+            if(enc==enc5.getPulses())   // Si el contador no ha cambiado
+                dir=1;          // cambiamos la dirección
+        }
+    }
+    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
+    {
+        mover(5,1,1);           // se mueven los motores 4 y 5 
+        wait(0.01);             // 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 home()
+{
+    
+}
+
+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);
+            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);
+            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);
+            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,0.8);             // movemos la pinza
+    enc=enc6.getPulses();       // leemos la posicion del encoder
+    do
+    {
+        wait(0.1);              // espera 0.1s
+        encOld=enc;             // guardamos la posicion antigua del encoder
+        enc=enc6.getPulses();   // leemos la posicion nueva del encoder
+    }while(enc!=encOld);        // mientras la posicion antigua sea distinta de la nueva seguimos leyendo
+    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;
+    }
+    
+}
+
+
+
+