Hexapod
/
PwmOut_HelloWorld
Proyecto Hexápodo
Fork of PwmOut_HelloWorld by
Diff: hexapod.cpp
- Revision:
- 15:91e78eb9cae9
- Parent:
- 14:f37214436441
- Child:
- 16:a82c32cbe63e
diff -r f37214436441 -r 91e78eb9cae9 hexapod.cpp --- a/hexapod.cpp Thu Oct 18 19:20:28 2018 +0000 +++ b/hexapod.cpp Tue Oct 23 15:01:24 2018 +0000 @@ -2,6 +2,8 @@ Serial my_serial(TX, RX); //Puerto serie +Thread thread; + 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)}, @@ -9,42 +11,56 @@ {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] = { {115,90,90}, +int mov_home[num_patas][num_servos] = { {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,90,90}, - {90,90,90}, - {60,90,90}, - {60,90,90}, - {90,90,90}, - {120,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] = { {120,90,90}, - {90,90,90}, - {60,90,90}, - {60,90,90}, - {90,90,90}, - {120,90,90}}; +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(1,'H'); + mueve_pata(2,'H'); + mueve_pata(3,'H'); + mueve_pata(4,'H'); + mueve_pata(5,'H'); + //mueve_hexapodo(0); //TIPO HOME wait_ms(3000); bool mov = false; - while(1){ //Hilo maestro "iría encolando un juego de comandos a cada pata" + while(1){ if (mov == true) { - mueve_pata(0,'A'); - // mueve_pata(1,'R'); + 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,'H'); - // mueve_pata(1,'A'); + mueve_pata(0,'A'); + mueve_pata(1,'R'); + mueve_pata(2,'A'); + mueve_pata(3,'R'); + mueve_pata(4,'A'); + mueve_pata(5,'R'); } + //mueve_hexapodo(1); mov = !mov; - wait_ms(1500); + wait_ms(2500); } } @@ -65,14 +81,22 @@ void mueve_pata(char pata, char pos){ switch(pos){ case 'A': //Avanza - //mueve_servo(&patas[pata][SERVO_COX], mov_avanza[pata][SERVO_COX]); - //mueve_servo(&patas[pata][SERVO_FEM], mov_avanza[pata][SERVO_FEM]); - //mueve_servo(&patas[pata][SERVO_TIB], mov_avanza[pata][SERVO_TIB]); + 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_COX], mov_retrocede[pata][SERVO_COX]); - //mueve_servo(&patas[pata][SERVO_FEM], mov_retrocede[pata][SERVO_FEM]); - //mueve_servo(&patas[pata][SERVO_TIB], mov_retrocede[pata][SERVO_TIB]); + 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]); @@ -80,9 +104,9 @@ 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]); + 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; } } @@ -95,6 +119,7 @@ type = 2 -> RETROCEDE ******************************************************************************************************************/ void mueve_hexapodo(int type){ + bool mov = false; if(type == HOME){ mueve_pata(0,'H'); mueve_pata(1,'H'); @@ -134,4 +159,5 @@ mueve_pata(4,'A'); mueve_pata(5,'R'); } + } } \ No newline at end of file