File content as of revision 12:e1d33e64797c:

#include "hexapod.h"

Serial my_serial(PC_6, PA_12); //Puerto serie

/*SoftPWM servo_C_B_R(PB_8); //Servo Coxa_Behind_Right
SoftPWM servo_F_B_R(PB_9); //Servo Femur_Behind_Right
SoftPWM servo_T_B_R(PB_7); //Servo Tibia_Behind_Right

SoftPWM servo_C_M_R(PA_0); //Servo Coxa_Medium_Right
SoftPWM servo_F_M_R(PA_1); //Servo Femur_Medium_Right
SoftPWM servo_T_M_R(PB_0); //Servo Tibia_Medium_Right

SoftPWM servo_C_F_R(PB_1); //Servo Coxa_Forward_Right
SoftPWM servo_F_F_R(PA_3); //Servo Femur_Forward_Right
SoftPWM servo_T_F_R(PA_2); //Servo Tibia_Forward_Right

SoftPWM servo_C_B_L(PA_5); //Servo Coxa_Behind_Left
SoftPWM servo_F_B_L(PA_6); //Servo Femur_Behind_Left
SoftPWM servo_T_B_L(PA_7); //Servo Tibia_Behind_Left

SoftPWM servo_C_M_L(PB_6); //Servo Coxa_Medium_Left
SoftPWM servo_F_M_L(PA_9); //Servo Femur_Medium_Left
SoftPWM servo_T_M_L(PA_8); //Servo Tibia_Medium_Left

SoftPWM servo_C_F_L(PB_10); //Servo Coxa_Forward_Left
SoftPWM servo_F_F_L(PB_3); //Servo Femur_Forward_Left
SoftPWM servo_T_F_L(PA_10); //Servo Tibia_Forward_Left
*/

SoftPWM pata1[] = {SoftPWM(PB_8),SoftPWM(PB_9),SoftPWM(PB_7)}; //{Coxa, Femur, Tibia} -> Behind Right
SoftPWM pata2[] = {SoftPWM(PA_0),SoftPWM(PA_1),SoftPWM(PB_0)}; //{Coxa, Femur, Tibia} -> Medium Right
SoftPWM pata3[] = {SoftPWM(PB_1),SoftPWM(PA_3),SoftPWM(PA_2)}; //{Coxa, Femur, Tibia} -> Forward Right
SoftPWM pata4[] = {SoftPWM(PA_5),SoftPWM(PA_6),SoftPWM(PA_7)}; //{Coxa, Femur, Tibia} -> Behind Left
SoftPWM pata5[] = {SoftPWM(PB_6),SoftPWM(PA_9),SoftPWM(PA_8)}; //{Coxa, Femur, Tibia} -> Medium Left
SoftPWM pata6[] = {SoftPWM(PB_10),SoftPWM(PB_3),SoftPWM(PA_10)}; //{Coxa, Femur, Tibia} -> Forward Left

int main() {
     pata3[1].period_us(20000); //20ms period, typical for analog RC servo
     bool mov = false;
     while(1){
        if (mov == true) {
            mueve_servo(&pata3[1], MOV_AV);
        } else {
            mueve_servo(&pata3[1], MOV_RE);
        }
        //mueve_pata('A');
         mov = !mov;
         wait_ms(1000); //Tiempo de espera entre movimiento y movimiento -> 250
         }
}

void mueve_servo(SoftPWM* pata, int grados){ //Función que realiza el movimiento de un servo
    int th=0;
    th = 560 + (1925*grados)/180;    //Límites del Servo [560 - 2485] -> OFFSET:1925
    pata->pulsewidth_us(th);
}

void mueve_pata1(char pos){ //Función que realiza el movimiento de una pata dependiendo de lo que se quiera hacer
    switch(pos){
        case 'A':    //Avanza
            mueve_servo(&pata1[0], MOV_AV);
            mueve_servo(&pata1[1], MOV_AV);
            mueve_servo(&pata1[2], MOV_AV);
            break;
        case 'R':    //Retrocede
            mueve_servo(&pata1[0], MOV_RE);
            mueve_servo(&pata1[1], MOV_RE);
            mueve_servo(&pata1[2], MOV_RE);
            break;
        case 'H':    //Home
            mueve_servo(&pata1[0], INICIO);
            mueve_servo(&pata1[1], INICIO);
            mueve_servo(&pata1[2], INICIO);
            break;
        case 'S':    //Sube
            mueve_servo(&pata1[1], MOV_AV); //Debe cambiarse el valor de MOV_AV
            mueve_servo(&pata1[2], MOV_AV); //Debe cambiarse el valor de MOV_AV
            break;
        case 'B':    //Baja
            mueve_servo(&pata1[1], MOV_RE); //Debe cambiarse el valor de MOV_RE
            mueve_servo(&pata1[2], MOV_RE); //Debe cambiarse el valor de MOV_RE
            break; 
        default:    //Home
            mueve_servo(&pata1[0], INICIO);
            mueve_servo(&pata1[1], INICIO);
            mueve_servo(&pata1[2], INICIO);
            break;
            //Falta Derecha e Izquierda
    }
}

void mueve_pata2(char pos){
    switch(pos){
        case 'A':    //Avanza
            mueve_servo(&pata2[0], MOV_AV);
            mueve_servo(&pata2[1], MOV_AV);
            mueve_servo(&pata2[2], MOV_AV);
            break;
        case 'R':    //Retrocede
            mueve_servo(&pata2[0], MOV_RE);
            mueve_servo(&pata2[1], MOV_RE);
            mueve_servo(&pata2[2], MOV_RE);
            break;
        case 'H':    //Home
            mueve_servo(&pata2[0], INICIO);
            mueve_servo(&pata2[1], INICIO);
            mueve_servo(&pata2[2], INICIO);
            break; 
        case 'S':    //Sube
            mueve_servo(&pata2[1], MOV_AV); //Debe cambiarse el valor de MOV_AV
            mueve_servo(&pata2[2], MOV_AV); //Debe cambiarse el valor de MOV_AV
            break;
        case 'B':    //Baja
            mueve_servo(&pata2[1], MOV_RE); //Debe cambiarse el valor de MOV_RE
            mueve_servo(&pata2[2], MOV_RE); //Debe cambiarse el valor de MOV_RE
            break; 
        default:    //Home
            mueve_servo(&pata2[0], INICIO);
            mueve_servo(&pata2[1], INICIO);
            mueve_servo(&pata2[2], INICIO);
            break;
            //Falta Derecha e Izquierda
    }
}

void mueve_pata3(char pos){
    switch(pos){
        case 'A':    //Avanza
            mueve_servo(&pata3[0], MOV_AV);
            mueve_servo(&pata3[1], MOV_AV);
            mueve_servo(&pata3[2], MOV_AV);
            break;
        case 'R':    //Retrocede
            mueve_servo(&pata3[0], MOV_RE);
            mueve_servo(&pata3[1], MOV_RE);
            mueve_servo(&pata3[2], MOV_RE);
            break;
        case 'H':    //Home
            mueve_servo(&pata3[0], INICIO);
            mueve_servo(&pata3[1], INICIO);
            mueve_servo(&pata3[2], INICIO);
            break; 
        case 'S':    //Sube
            mueve_servo(&pata3[1], MOV_AV); //Debe cambiarse el valor de MOV_AV
            mueve_servo(&pata3[2], MOV_AV); //Debe cambiarse el valor de MOV_AV
            break;
        case 'B':    //Baja
            mueve_servo(&pata3[1], MOV_RE); //Debe cambiarse el valor de MOV_RE
            mueve_servo(&pata3[2], MOV_RE); //Debe cambiarse el valor de MOV_RE
            break; 
        default:    //Home
            mueve_servo(&pata3[0], INICIO);
            mueve_servo(&pata3[1], INICIO);
            mueve_servo(&pata3[2], INICIO);
            break;
            //Falta Derecha e Izquierda
    }
}

void mueve_pata4(char pos){
    switch(pos){
        case 'A':    //Avanza
            mueve_servo(&pata4[0], MOV_AV);
            mueve_servo(&pata4[1], MOV_AV);
            mueve_servo(&pata4[2], MOV_AV);
            break;
        case 'R':    //Retrocede
            mueve_servo(&pata4[0], MOV_RE);
            mueve_servo(&pata4[1], MOV_RE);
            mueve_servo(&pata4[2], MOV_RE);
            break;
        case 'H':    //Home
            mueve_servo(&pata4[0], INICIO);
            mueve_servo(&pata4[1], INICIO);
            mueve_servo(&pata4[2], INICIO);
            break; 
        case 'S':    //Sube
            mueve_servo(&pata4[1], MOV_AV); //Debe cambiarse el valor de MOV_AV
            mueve_servo(&pata4[2], MOV_AV); //Debe cambiarse el valor de MOV_AV
            break;
        case 'B':    //Baja
            mueve_servo(&pata4[1], MOV_RE); //Debe cambiarse el valor de MOV_RE
            mueve_servo(&pata4[2], MOV_RE); //Debe cambiarse el valor de MOV_RE
            break; 
        default:    //Home
            mueve_servo(&pata4[0], INICIO);
            mueve_servo(&pata4[1], INICIO);
            mueve_servo(&pata4[2], INICIO);
            break;
            //Falta Derecha e Izquierda
    }
}

void mueve_pata5(char pos){
    switch(pos){
        case 'A':    //Avanza
            mueve_servo(&pata5[0], MOV_AV);
            mueve_servo(&pata5[1], MOV_AV);
            mueve_servo(&pata5[2], MOV_AV);
            break;
        case 'R':    //Retrocede
            mueve_servo(&pata5[0], MOV_RE);
            mueve_servo(&pata5[1], MOV_RE);
            mueve_servo(&pata5[2], MOV_RE);
            break;
        case 'H':    //Home
            mueve_servo(&pata5[0], INICIO);
            mueve_servo(&pata5[1], INICIO);
            mueve_servo(&pata5[2], INICIO);
            break; 
        case 'S':    //Sube
            mueve_servo(&pata5[1], MOV_AV); //Debe cambiarse el valor de MOV_AV
            mueve_servo(&pata5[2], MOV_AV); //Debe cambiarse el valor de MOV_AV
            break;
        case 'B':    //Baja
            mueve_servo(&pata5[1], MOV_RE); //Debe cambiarse el valor de MOV_RE
            mueve_servo(&pata5[2], MOV_RE); //Debe cambiarse el valor de MOV_RE
            break; 
        default:    //Home
            mueve_servo(&pata5[0], INICIO);
            mueve_servo(&pata5[1], INICIO);
            mueve_servo(&pata5[2], INICIO);
            break;
            //Falta Derecha e Izquierda
    }
}

void mueve_pata6(char pos){
    switch(pos){
        case 'A':    //Avanza
            mueve_servo(&pata6[0], MOV_AV);
            mueve_servo(&pata6[1], MOV_AV);
            mueve_servo(&pata6[2], MOV_AV);
            break;
        case 'R':    //Retrocede
            mueve_servo(&pata6[0], MOV_RE);
            mueve_servo(&pata6[1], MOV_RE);
            mueve_servo(&pata6[2], MOV_RE);
            break;
        case 'H':    //Home
            mueve_servo(&pata6[0], INICIO);
            mueve_servo(&pata6[1], INICIO);
            mueve_servo(&pata6[2], INICIO);
            break; 
        case 'S':    //Sube
            mueve_servo(&pata6[1], MOV_AV); //Debe cambiarse el valor de MOV_AV
            mueve_servo(&pata6[2], MOV_AV); //Debe cambiarse el valor de MOV_AV
            break;
        case 'B':    //Baja
            mueve_servo(&pata6[1], MOV_RE); //Debe cambiarse el valor de MOV_RE
            mueve_servo(&pata6[2], MOV_RE); //Debe cambiarse el valor de MOV_RE
            break;
        default:    //Home
            mueve_servo(&pata6[0], INICIO);
            mueve_servo(&pata6[1], INICIO);
            mueve_servo(&pata6[2], INICIO);
            break;
            //Falta Derecha e Izquierda
    }
}

void hexapod_mov(int type){ //Función que hace el movimiento del hexápodo dependiendo de lo que se quiera hacer
    if(type == ARRIBA){
        mueve_pata1('S');
        mueve_pata2('S');
        mueve_pata3('S');
        mueve_pata4('S');
        mueve_pata5('S');
        mueve_pata6('S');
    }else if(type == ABAJO){
        mueve_pata1('B');
        mueve_pata2('B');
        mueve_pata3('B');
        mueve_pata4('B');
        mueve_pata5('B');
        mueve_pata6('B');
    }else if(type == HOME){
        mueve_pata1('H');
        mueve_pata2('H');
        mueve_pata3('H');
        mueve_pata4('H');
        mueve_pata5('H');
        mueve_pata6('H');
    }else if(type == AVANZA){
            //Rellenar el movimiento con las patas
    }else if(type == RETROCEDE){
            //Rellenar el movimiento con las patas
    }else if(type == DERECHA){
            //Rellenar el movimiento con las patas
    }else if(type == IZQUIERDA){
            //Rellenar el movimiento con las patas
    }    
}