Martin Smith
/
InverseKinematics2
Diff: GetAngles.cpp
- Revision:
- 0:fdc9474c08b9
diff -r 000000000000 -r fdc9474c08b9 GetAngles.cpp --- /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