From 8e66e26bb97a2abe6128ff24395cef252a22af1b Mon Sep 17 00:00:00 2001 From: MangaValk Date: Fri, 2 Dec 2011 22:03:05 +0100 Subject: [PATCH] FireFly 2011: tuned leg positions, walk sequence and IK From 'hexapod V2 29-12-11.rar' (main file was hexapod.pde). --- .../hexapod_oud/InverseKinemetics.cpp | 20 ++++++---- .../2011-nunchuk/hexapod_oud/hexapod_oud.pde | 4 +- .../2011-nunchuk/hexapod_oud/sequnces.h | 40 +++++-------------- 3 files changed, 24 insertions(+), 40 deletions(-) diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.cpp b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.cpp index 372da5f..c558a18 100644 --- a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.cpp +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.cpp @@ -9,7 +9,10 @@ void InverseKinemetics::RL3 (int i) float InverseKinemetics::ZX (int i) { RL3(i); - float temp = degrees(acos((float)COR_2_IK[2+(i*3)] / l3)); + if (COR_2_IK[2+(i*3)] != 0) + float temp = degrees(acos((float)COR_2_IK[2+(i*3)] / l3)); + else + float temp = 90; // Math Error Protection. return temp; } @@ -19,19 +22,19 @@ float InverseKinemetics::TT (int i) int y = pow(abs(COR_2_IK[1+(i*3)]), 2); int z = pow(abs(COR_2_IK[2+(i*3)]), 2); s = sqrt(x + y + z); - + if (s > (l1 + l2) ) R[i] = 3; - + float t1 = degrees(acos((pow(s, 2) + pow(COR_2_IK[1+(i*3)], 2) - pow(l3, 2)) - / (2 * s * COR_2_IK[1+(i*3)]))); - + / (2 * s * COR_2_IK[1+(i*3)]))); + float t2 = degrees(acos((pow(s, 2) + pow(l1, 2) - pow(l2, 2)) - / (2 * s * l1))); - + / (2 * s * l1))); + float temp = (int)t1 + t2; //Serial.print("DEBUG: "); Serial.print(t1); Serial.print(", "); Serial.print(t2); Serial.print(", "); Serial.println(temp); - + return temp; } @@ -150,4 +153,5 @@ int InverseKinemetics::CheckForErrorAng() + diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/hexapod_oud.pde b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/hexapod_oud.pde index f584639..0866ea2 100644 --- a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/hexapod_oud.pde +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/hexapod_oud.pde @@ -101,8 +101,8 @@ void loop() COR_2_IK[2+(2*3)] = 10;/// COR_2_IK[2+(3*3)] = -10;// COR_2_IK[2+(5*3)] = 10;/// - COR_2_IK[0+(1*3)] = 15;// z side body - COR_2_IK[0+(4*3)] = 15; + COR_2_IK[0+(1*3)] = 10;// z side body + COR_2_IK[0+(4*3)] = 10; } else { diff --git a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/sequnces.h b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/sequnces.h index 84d253f..0f0fbd6 100644 --- a/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/sequnces.h +++ b/2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/sequnces.h @@ -2,37 +2,17 @@ // Hexapod seqencer. int TriPoD_Size = 8; -// zij, laag, achter -// 15, 15, -15, 15, 15, 0, 15, 15, 15, 15, 15, -15, 15, 15, 0, 15, 15, 15; + int TriPoD[8][18] = { - {// -15 is voor - 15, 2, 0, 15, 15, 0, 15, 2, 0, 15, 15, 0,15, 2, 0, 15, 15, 0 }//miden*hoog, miden*laag - , - { - 15, 2, -2, 15, 15, 2, 15, 2, -2, 15, 15, 2,15, 2, -2, 15, 15, 2 }// voor*hoog, achter*laag - , - { - 15, 15, -2, 15, 15, 2, 15, 15, -2, 15, 15, 2,15, 15, -2, 15, 15, 2 }// laag, niks - , - { - 15, 15, -2, 15, 2, 2, 15, 15, -2, 15, 2, 2,15, 15, -2, 15, 2, 2 }// niks, hoog - , - { - 15, 15, 0, 15, 2, 0, 15, 15, 0, 15, 2, 0,15, 15, 0, 15, 2, 0 }//midden*laag, miden*hoog - , - { - 15, 15, 2, 15, 2, -2, 15, 15, 2, 15, 2, -2,15, 15, 2, 15, 2, -2 }//voor-laag, achter/hoog - , - { - 15, 15, 2, 15, 15, -2, 15, 15, 2, 15, 15, -2,15, 15, 2, 15, 15, -2 }// niks, laag - , - { - 15, 2, 2, 15, 15, -2, 15, 2, 2, 15, 15, -2,15, 2, 2, 15, 15, -2 }// hoog, niks -}; - - - - + {10,10, 0,10,15, 0,10,10, 0,10,15, 0,10,10, 0,10,15, 0}, + {10,10,-2,10,15, 2,10,10,-2,10,15, 3,10,10,-2,10,15, 2}, + {10,15,-2,10,15, 2,10,15,-2,10,15, 3,10,15,-2,10,15, 2}, + {10,15,-2,10,10, 2,10,15,-2,10,10, 3,10,15,-2,10,10, 2}, + {10,15, 0,10,10, 0,10,15, 0,10,10, 0,10,15, 0,10,10, 0}, + {10,15, 2,10,10,-2,10,15, 2,10,10,-2,10,15, 2,10,10,-2}, + {10,15, 2,10,15,-2,10,15, 2,10,15,-2,10,15, 2,10,15,-2}, + {10,10, 2,10,15,-2,10,10, 2,10,15,-2,10,10, 2,10,15,-2} + };