Diff: hexapod.cpp

Revision:

15:91e78eb9cae9

Parent:

14:f37214436441

Child:

16:a82c32cbe63e

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