Martin Smith
/
InverseKinematics2
GetAngles.cpp
- Committer:
- ms523
- Date:
- 2011-01-28
- Revision:
- 0:fdc9474c08b9
File content as of revision 0:fdc9474c08b9:
#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); }