File content as of revision 3:0b3d5b1850aa:

#include "mbed.h"           // declaramos las librerias de funciones 
#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 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
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 
char dato=0;
float V=1;

// declaramos el prototipo de las funciones que utilizaremos
void enviar_bt();               // 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(int);             // 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
void movCinta();                // prototipo de la funcion que avanzara la cinta hasta detectar un objeto
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.
    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.2s

    sethome();              // establecemos la posicion de referencia
    
    while(1)
    {
        if(bt.readable()>0)             // si hay un dato disponible:
        {
            dato = bt.getc();           // leemos un caracter
            if (dato<=10 && dato>=1)    // si tiene un valor entre 1 y 10 lo asignamos como velocidad
                V=map(dato,1,10,0.6,1);
            switch(dato)
            {   
                case 'A':       // si el caracter es A se mueve el motor 1 al derecho
                mover(1,1,V);
                wait(0.2);
                frenar(1);
                break;
                case 'B':       // si el caracter es B se mueve el motor 1 al inverso
                mover(1,-1,V);
                wait(0.2);
                frenar(1);
                break;
                case 'C':       // si el caracter es C se mueve el motor 2 al derecho
                mover(2,1,V);
                wait(0.2);
                frenar(2);
                break;
                case 'D':       // si el caracter es D se mueve el motor 2 al inverso
                mover(2,-1,V);
                wait(0.2);
                frenar(2);
                break;
                case 'E':       // si el caracter es E se mueve el motor 3 al derecho
                mover(3,1,V);
                wait(0.2);
                frenar(3);
                break;
                case 'F':       // si el caracter es F se mueve el motor 3 al inverso
                mover(3,-1,V);
                wait(0.2);
                frenar(3);
                break;
                case 'G':       // si el caracter es G se mueve el motor 4 Y 5 al derecho
                mover(14,1,V);
                wait(0.2);
                frenar(14);
                break;
                case 'H':       // si el caracter es H se mueve el motor 4 Y 5 al inverso
                mover(14,-1,V);
                wait(0.2);
                frenar(14);
                break;
                case 'I':       // si el caracter es I se mueve el motor 4 Y 5 al derecho
                mover(15,1,V);
                wait(0.2);
                frenar(15);
                break;
                case 'J':       // si el caracter es J se mueve el motor 4 Y 5 al inverso
                mover(15,-1,V);
                wait(0.2);
                frenar(15);
                break;
                case 'K':       // si el caracter es K se abre la pinza
                pinza(-1);
                break;
                case 'L':       // si el caracter es L se cierra la pinza
                pinza(1);
                break;
                case 'M':       // si el caracter es M se mueve a la posicion home
                posicion(0);
                pinza(-1);
                break;
                case 'N':       // si el caracter es N se mueve a la posicion 1
                posicion(1);
                break;
                case 'O':       // si el caracter es O se mueve a la posicion 2
                posicion(2);
                break;
                case 'P':       // si el caracter es P se mueve a la posicion 3
                posicion(3);
                break;
                case 'Q':       // si el caracter es Q se mueve a la posicion 4
                posicion(4);
                break;
                case 'R':       // si el caracter es R se mueve a la posicion 5
                posicion(5);
                break;
                case 'S':       // si el caracter es S se mueve a la posicion 6
                posicion(6);
                break;
                case 'T':       // si el caracter es T se mueve a la posicion 7
                posicion(7);
                break;
                case 'U':       // si el caracter es U se mueve a la posicion 8
                posicion(8);
                break;
                case 'V':       // si el caracter es V se mueve a la posicion 9
                posicion(9);
                break;
            }
        }
    }
}

float map(float x, float in_min, float in_max, float out_min, float out_max)    // funcion que redimensiona los parametros
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void enviar_bt()            // definimos la funcion que ejecutara el ticker
{
    pulsos[1]=enc1.getPulses();     // cargamos los contadores de los encoders
    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 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
        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
    }
    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
            {
                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
    
    // 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
            {
                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
    
    // 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(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
            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(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 
        wait(0.01);             // esperamos 0.01s
    }
    frenar(15);                  // frenamos los motores
    
    pinza(-1);                  // abrimos la pinza
    
    // ponemos el contador de los encoders a 0
    frenar(0);                  // frenamos todos los motoes
    enc1.reset();               // reseteamos los encoders
    enc2.reset();
    enc3.reset();
    enc4.reset();
    enc5.reset();
    enc6.reset();
    led2=0;
}

void posicion(int pos)
{
    frenar(0);                      // nos aseguramos de que los motores estan parados
    pulsos[1]=enc1.getPulses();     // leemos los valores de los conadores de los encoders
    pulsos[2]=enc2.getPulses();
    pulsos[3]=enc3.getPulses();
    pulsos[4]=enc4.getPulses();
    pulsos[5]=enc5.getPulses();
    int dir[6]={0,0,0,0,0,0};       // declaramos un array para las direcciones
    int flag[6]={1,1,1,1,1,1};      // declaramos un array para banderas
    int i;
    for (i=1;i<6;i++)               // establecemos la direccion a la que tiene que ir cada motor 
    {
        if (posiciones[pos][i]<pulsos[i])
            dir[i]=-1;
        else if (posiciones[pos][i]>pulsos[i])
            dir[i]=1;
        else if (posiciones[pos][i]==pulsos[i])
            flag[i]=0;
    }
    while (flag[1]+flag[2]+flag[3]+flag[4]+flag[5]) // mientras que alguna de las banderas este activada:
    {
        for (i=1;i<6;i++)       // repetimos para cada uno de los motores
        {
            if (flag[i]==1)     // si la bandera del motor i esta activa
            {
                mover(i,dir[i],V);  // movemos el motor en la direccion establecida 
                if (dir[i]==1 && pulsos[i]>=posiciones[pos][i]) // si ha llegado a la posicion deseada
                {
                    frenar(i);      // frenamos el motor i
                    flag[i]=0;      // desactivamos su bandera
                }
                if (dir[i]==-1 && pulsos[i]<=posiciones[pos][i]) // si ha llegado a la posicion deseada
                {
                    frenar(i);      // frenamos el motor i
                    flag[i]=0;      // desactivamos su bandera
                }
            }
        }
        pulsos[1]=enc1.getPulses();     // leemos la nueva posicion de los encoders
        pulsos[2]=enc2.getPulses();
        pulsos[3]=enc3.getPulses();
        pulsos[4]=enc4.getPulses();
        pulsos[5]=enc5.getPulses();
    }
wait(0.1);              // esperamos 0.1s
}

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 
            m4.speed(dir*v);
            break;
            case 5:             // en caso de que el valor sea 5 movemos el motor 5
            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;
            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)
            m4.speed(dir*v);
            m5.speed(dir*v);
        }
    }
    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 el motor 4 
        m4.brake();
        break;
        case 5:         // en caso de que el valor sea 5 frenamos el motor 5
        m5.brake();
        break;
        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
        m4.brake();
        m5.brake();
        break;
        case 15:         // en caso de que el valor sea 5 frenamos los motores 4 y 5
        m4.brake();
        m5.brake();
    }
}
void movCinta()
{
    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
}