FireFly 2011: tuned leg positions, walk sequence and IK

From 'hexapod V2 29-12-11.rar' (main file was hexapod.pde).

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()
 
 
 
+