File content as of revision 16:a82c32cbe63e:

#include "hexapod.h"

Serial my_serial(TX, RX); //Puerto serie

Thread thread[6];

SoftPWM patas[num_patas][num_servos] = {{SoftPWM(coxa_1),SoftPWM(femur_1),SoftPWM(tibia_1)},
                                        {SoftPWM(coxa_2),SoftPWM(femur_2),SoftPWM(tibia_2)},
                                        {SoftPWM(coxa_3),SoftPWM(femur_3),SoftPWM(tibia_3)},
                                        {SoftPWM(coxa_4),SoftPWM(femur_4),SoftPWM(tibia_4)},
                                        {SoftPWM(coxa_5),SoftPWM(femur_5),SoftPWM(tibia_5)},
                                        {SoftPWM(coxa_6),SoftPWM(femur_6),SoftPWM(tibia_6)}};

int mov_home[num_patas][num_servos] = { {120,90,90},
                                        {85,90,90},
                                        {60,90,90},
                                        {60,90,90},
                                        {95,90,90},
                                        {130,90,90}};


                                        /*{120,90,90},
                                        {90,90,90},
                                        {60,90,90},
                                        {60,90,90},
                                        {90,90,90},
                                        {115,90,90}};*/
                                        
int mov_avanza[num_patas][num_servos] = { {135,135,135},
                                          {105,135,135},
                                          {75,135,135},
                                          {45,55,55},
                                          {75,55,55},
                                          {100,55,55}};

int mov_retrocede[num_patas][num_servos] = { {105,90,90},
                                             {75,90,90},
                                             {45,90,90},
                                             {75,90,90},
                                             {105,90,90},
                                             {130,90,90}};

int main() {
     mueve_pata(0,'H');
     mueve_pata(1,'H');
     mueve_pata(2,'H');
     mueve_pata(3,'H');
     mueve_pata(4,'H');
     mueve_pata(5,'H');
     wait_ms(3000);
     //thread[0].start(callback(mueve_pata, patas[0][0], 'A'));
     bool mov = false;
     while(1){
        if (mov == true) {
            /*mueve_pata(0,'R');
            mueve_pata(1,'A');
            mueve_pata(2,'R');
            mueve_pata(3,'A');
            mueve_pata(4,'R');
            mueve_pata(5,'A');
        } else {
            mueve_pata(0,'A');
            mueve_pata(1,'R');
            mueve_pata(2,'A');
            mueve_pata(3,'R');
            mueve_pata(4,'A');
            mueve_pata(5,'R');*/
        }
         mov = !mov;
         wait_ms(2500);
         }
}


/*******************************************************************************************
Función mueve_servo -> Realiza el movimiento del servo dependiendo del grado que se le pase
*******************************************************************************************/
void mueve_servo(SoftPWM* servo, int grados){
    float th=0;
    th = 565 + (1920/180)*grados;
    servo->pulsewidth_us(th);
}


/******************************************************************************************************************
Función mueve_pata -> Realiza el movimiento de la pata que se le pasa de entrada e indicando el tipo de movimiento
******************************************************************************************************************/
void mueve_pata(char pata, char pos){
    switch(pos){
        case 'A':    //Avanza
            mueve_servo(&patas[pata][SERVO_TIB], mov_avanza[pata][SERVO_TIB]);
            mueve_servo(&patas[pata][SERVO_FEM], mov_avanza[pata][SERVO_FEM]);
            wait_ms(175);
            mueve_servo(&patas[pata][SERVO_COX], mov_avanza[pata][SERVO_COX]);
            wait_ms(175);
            mueve_servo(&patas[pata][SERVO_FEM], mov_home[pata][SERVO_FEM]);
            mueve_servo(&patas[pata][SERVO_TIB], mov_home[pata][SERVO_TIB]);
            break;
        case 'R':    //Retrocede
            mueve_servo(&patas[pata][SERVO_TIB], mov_avanza[pata][SERVO_TIB]);
            mueve_servo(&patas[pata][SERVO_FEM], mov_avanza[pata][SERVO_FEM]);
            wait_ms(175);
            mueve_servo(&patas[pata][SERVO_COX], mov_retrocede[pata][SERVO_COX]);
            wait_ms(175);
            mueve_servo(&patas[pata][SERVO_FEM], mov_home[pata][SERVO_FEM]);
            mueve_servo(&patas[pata][SERVO_TIB], mov_home[pata][SERVO_TIB]);
            break;
        case 'H':    //Home
            mueve_servo(&patas[pata][SERVO_COX], mov_home[pata][SERVO_COX]);
            mueve_servo(&patas[pata][SERVO_FEM], mov_home[pata][SERVO_FEM]);
            mueve_servo(&patas[pata][SERVO_TIB], mov_home[pata][SERVO_TIB]);
            break;
        default:    //Home
            mueve_servo(&patas[pata][SERVO_COX], mov_home[pata][SERVO_COX]);
            mueve_servo(&patas[pata][SERVO_FEM], mov_home[pata][SERVO_FEM]);
            mueve_servo(&patas[pata][SERVO_TIB], mov_home[pata][SERVO_TIB]);
            break;
    }
}


/******************************************************************************************************************
Función mueve_hexapodo -> Realiza el movimiento del hexapodo completo dependiendo del tipo elegido
        type = 0 -> HOME
        type = 1 -> AVANZA
        type = 2 -> RETROCEDE
******************************************************************************************************************/
void mueve_hexapodo(int type){
    bool mov = false;
    if(type == HOME){
        mueve_pata(0,'H');
        mueve_pata(1,'H');
        mueve_pata(2,'H');
        mueve_pata(3,'H');
        mueve_pata(4,'H');
        mueve_pata(5,'H');
    }else if(type == AVANZA){
        if (mov == true) {
            mueve_pata(0,'A');
            mueve_pata(1,'R');
            mueve_pata(2,'A');
            mueve_pata(3,'R');
            mueve_pata(4,'A');
            mueve_pata(5,'R');
        } else {
            mueve_pata(0,'R');
            mueve_pata(1,'A');
            mueve_pata(2,'R');
            mueve_pata(3,'A');
            mueve_pata(4,'R');
            mueve_pata(5,'A');
        }
    }else if(type == RETROCEDE){
        if (mov == true) {
            mueve_pata(0,'R');
            mueve_pata(1,'A');
            mueve_pata(2,'R');
            mueve_pata(3,'A');
            mueve_pata(4,'R');
            mueve_pata(5,'A');
        } else {
            mueve_pata(0,'A');
            mueve_pata(1,'R');
            mueve_pata(2,'A');
            mueve_pata(3,'R');
            mueve_pata(4,'A');
            mueve_pata(5,'R');
        }
    }
}