Hexapod
/
PwmOut_HelloWorld
Proyecto Hexápodo
Fork of PwmOut_HelloWorld by
Diff: hexapod.cpp
- Revision:
- 9:8e25ad440b2e
- Parent:
- 8:b0a6c16e781b
- Child:
- 10:88d5a9711af8
diff -r b0a6c16e781b -r 8e25ad440b2e hexapod.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hexapod.cpp Tue Oct 09 18:20:43 2018 +0000 @@ -0,0 +1,229 @@ +#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], 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 + } +} \ No newline at end of file