File content as of revision 0:8a7c1e92d067:

/********************************************************************************
 This function finds E.R.I.C.'s joint angles for a given foot position
 Notebook: http://mbed.org/users/ms523/notebook/eric-leg-inverse-kinematics/
********************************************************************************/
#include "FK.h"

Joint_Angles IK_Engine(int X, int Y, int Z, char legs){

    // Adjust X to account for the X-axis offset
        if(X > 0)
            X = X - X_OFFSET;
        else
            X = X + X_OFFSET;     
            
    // Calculate Leg length
        float A = X*X+Z*Z-B*B;
        A = sqrt(A);
        
    // Calculate beta
        float beta = A/B;
        beta = atan(beta);
        beta = beta * 180 / PI;
        
    // Calculate C
        float C = X*X+Z*Z;
        C = sqrt(C);
         
    // Calculate gamma
        float gamma = (float)Z / C;
        gamma = acos(gamma);
        gamma = gamma * 180 / PI;
        
    // Calculate alpha (i.e. lateral servo angle)
        float alpha = gamma - beta;

    // Calculate the length of the leg
        float D = A*A+Y*Y;
        D = sqrt(D);

        // Work out phi (i.e. knee servo angle)
        float phi;
        if(legs){           // Fore legs selected
            phi = ULNA*ULNA+RADIUS*RADIUS-D*D;
            phi = phi / (2*ULNA*RADIUS);
            phi = acos(phi);
            phi = phi * 180 / PI;
            phi = 360 - phi;            // Because hind legs bend backwards
        }
        else{               // Hind legs selected
            phi = FEMUR*FEMUR+TIBIA*TIBIA-D*D;
            phi = phi / (2*FEMUR*TIBIA);
            phi = acos(phi);
            phi = phi * 180 / PI;
        }

    // Work out epsilon
        float epsilon;
        if(legs){           // Fore legs selected
            epsilon = ULNA*ULNA-RADIUS*RADIUS+D*D;
            epsilon = epsilon / (2*D*ULNA);
        }
        else{               // Hind legs selected
            epsilon = FEMUR*FEMUR-TIBIA*TIBIA+D*D;
            epsilon = epsilon / (2*D*FEMUR);
        }
        epsilon = acos(epsilon);
        epsilon = epsilon * 180 / PI;
        
    // Work out delta
        float delta = (float) Y/ A;                   
        delta = atan(delta);
        delta = delta * 180 / PI;
        
    // Work out theta (i.e. hip servo angle)
        float theta;
        if(legs){           // Fore legs selected
            theta = 180 - delta + epsilon;      // Because fore legs bend backwards
        }
        else{               // Hind legs selected
            theta = 180 - delta - epsilon;
        }
        
    // Return calculated joint angles   
        Joint_Angles angles;    // Local variable 
        angles.lateral = alpha;
        angles.hip = theta;
        angles.knee = phi;
        return(angles);   
 }