Diff: GetAngles.cpp

Revision:

0:fdc9474c08b9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GetAngles.cpp	Fri Jan 28 19:45:20 2011 +0000
@@ -0,0 +1,32 @@
+#include "ServoAngles.h"
+
+LegAngles GetAngles(int X, int Y) {
+// Begin by working out L
+        float L = (X*X) + (Y*Y);
+        L = sqrt(L);
+
+// Work out the Knee angle
+        float Knee = (FEMUR*FEMUR)+(TIBIA*TIBIA)-(L*L);
+        Knee = Knee / (2*FEMUR*TIBIA);
+        Knee = acos(Knee);
+
+// Work out Alpha
+        float Alpha = (FEMUR*FEMUR)+(L*L)-(TIBIA*TIBIA);
+        Alpha = Alpha / (2*FEMUR*L);
+        Alpha = acos(Alpha);
+
+// Work out Beta
+        float Beta = (float) X/(float) Y;
+        Beta = atan(Beta);
+
+// Finally work out the Hip angle
+        float Hip = PI - Alpha - Beta;
+
+// Convert the angles to degrees...
+        LegAngles Angles;
+        Angles.Knee = Knee * 180 / PI - 30;
+        Angles.Hip = 360 - (Hip * 180 / PI) - 30;
+
+// Return the hip and knee angles...
+        return(Angles);       
+}
\ No newline at end of file