File content as of revision 8:b0a6c16e781b:

#include "mbed.h"
#include "SoftPWM.h"
#include "Servo.h"

#define MOV_AV 180    //En grados
#define INICIO 90     //En grados
#define MOV_RE 0      //En grados

#define ARRIBA 0
#define ABAJO 1
#define HOME 2
#define AVANZA 3
#define RETROCEDE 4
#define DERECHA 5
#define IZQUIERDA 6


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

void mueve_servo(SoftPWM* pata, int grados);
void mueve_pata1(char pos);
void mueve_pata2(char pos);
void mueve_pata3(char pos);
void mueve_pata4(char pos);
void mueve_pata5(char pos);
void mueve_pata6(char pos);
void hexapod_mov(int type);

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], AVANZA);
        } else {
            mueve_servo(&pata3[1], HOME);
        }
        //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[0], INICIO);
            mueve_servo(&pata1[1], INICIO);
            mueve_servo(&pata1[2], INICIO);
            break;    
        default:    //Home
            mueve_servo(&pata1[0], INICIO);
            mueve_servo(&pata1[1], INICIO);
            mueve_servo(&pata1[2], INICIO);
            break;
    }
}

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; 
        default:    //Home
            mueve_servo(&pata2[0], INICIO);
            mueve_servo(&pata2[1], INICIO);
            mueve_servo(&pata2[2], INICIO);
            break;           
    }
}

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; 
        default:    //Home
            mueve_servo(&pata3[0], INICIO);
            mueve_servo(&pata3[1], INICIO);
            mueve_servo(&pata3[2], INICIO);
            break;           
    }
}

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; 
        default:    //Home
            mueve_servo(&pata4[0], INICIO);
            mueve_servo(&pata4[1], INICIO);
            mueve_servo(&pata4[2], INICIO);
            break;           
    }
}

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; 
        default:    //Home
            mueve_servo(&pata5[0], INICIO);
            mueve_servo(&pata5[1], INICIO);
            mueve_servo(&pata5[2], INICIO);
            break;           
    }
}

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; 
        default:    //Home
            mueve_servo(&pata6[0], INICIO);
            mueve_servo(&pata6[1], INICIO);
            mueve_servo(&pata6[2], INICIO);
            break;           
    }
}

void hexapod_mov(int type){ //Función que hace el movimiento del hexápodo dependiendo de lo que se quiera hacer
    if(type == ARRIBA){
            //Rellenar el movimiento con las patas
    }else if(type == ABAJO){
            //Rellenar el movimiento con las patas
    }else if(type == HOME){
            //Rellenar el movimiento con las patas
    }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
    }    
}