Diff: hexapod.cpp

Revision:

9:8e25ad440b2e

Parent:

8:b0a6c16e781b

Child:

10:88d5a9711af8

--- /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