Hexapod
/
PwmOut_HelloWorld
Proyecto Hexápodo
Fork of PwmOut_HelloWorld by
hexapod.cpp
- Committer:
- Francisco_12
- Date:
- 2018-10-24
- Revision:
- 16:a82c32cbe63e
- Parent:
- 15:91e78eb9cae9
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'); } } }