Martin Smith / Mbed 2 deprecated InverseKinematics2

Dependencies:   mbed

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);       
}