File content as of revision 1:031a0c78d8d6:

#include "mbed.h"
#include "quadrapod_defs.h"
#include "ArthropodIK.h"

float sq(float n1){
    float res = n1 * n1;
    return res;
    }
    
    
const float ArthropodSolver::LEG_ANGLES[4] = {0.78539816339, 2.35619449019, -2.35619449019, -0.78539816339, };


ArthropodSolver::ArthropodSolver()  // Constuctor initializes thing. 
{ 


}


 leg_angles_t ArthropodSolver::SolveLeg(float LegTargetXYZ[], int LegNum)
    {
        leg_angles_t angles_out; 
        
        angles_out.gamma = atan2(LegTargetXYZ[1], LegTargetXYZ[0]) - LEG_ANGLES[LegNum]; 
        
        float lp = sqrt(sq(LegTargetXYZ[0]) + sq(LegTargetXYZ[1]));
        
        float L2 = sq(LegTargetXYZ[2]) + sq(lp - COXA_L);
        float a1 = acos(-LegTargetXYZ[2] / sqrt(L2));
        float a2 = acos((sq(TIBIA_L)-sq(FEMUR_L)-L2)/(-2*FEMUR_L*sqrt(L2)));
        angles_out.alpha = a1 + a2;
        angles_out.beta = acos((-sq(TIBIA_L)-sq(FEMUR_L)+L2)/(-2*FEMUR_L*TIBIA_L));
        
        
        // IK code, the most basic section. 
        
        
        return angles_out; // THIS WILL OVERWRITE WITH MULTIPLE CALLS!!!!!!       
}
    


float * ArthropodSolver::SolveBody(sixDOF_t BodyTarget6D, float LegPriorPos[], int LegNum) {
        
        static float foot_pos_out[3];
        float xp= 0, yp = 0; 
        
        float foot_pos_inter[3]; 
        float foot_pos_new[3]; 
        switch (LegNum) {                   // Get the coordinates relative to the robot center rather than the leg axis. 
            case 0: 
                xp = HIP_DISP_ORTHO;
                yp = HIP_DISP_ORTHO;
                break;
            case 1: 
                xp = -HIP_DISP_ORTHO;
                yp = HIP_DISP_ORTHO;
                break;
            case 2: 
                xp = -HIP_DISP_ORTHO;
                yp = -HIP_DISP_ORTHO;
                break;
            case 3: 
                xp = HIP_DISP_ORTHO;
                yp = -HIP_DISP_ORTHO;
                break;
            }            
        foot_pos_new[0] = LegPriorPos[0] + xp + BodyTarget6D.xyz[0];
        foot_pos_new[1] = LegPriorPos[1] + yp + BodyTarget6D.xyz[1];
        foot_pos_new[2] = LegPriorPos[2]  +  BodyTarget6D.xyz[2];
        
        YawXform(foot_pos_new,foot_pos_inter,BodyTarget6D.ypr[0]); // apply yaw transformation first. 
        PitchXform(foot_pos_inter, foot_pos_new,BodyTarget6D.ypr[1]);
        RollXform(foot_pos_new, foot_pos_inter,BodyTarget6D.ypr[2]);
        
        foot_pos_out[0] = foot_pos_inter[0] - xp  - BodyTarget6D.xyz[0];
        foot_pos_out[1] = foot_pos_inter[1] - yp - BodyTarget6D.xyz[0];
        foot_pos_out[2] = foot_pos_inter[2] - BodyTarget6D.xyz[0];
        
       
       
       
        
        return foot_pos_out; 
}
    
    
    
    
float * ArthropodSolver::SolveLegFwd(leg_angles_t LegAngles, int LegNum){
        static float footpos_out[3]; //x y z
             
         // code
         
        float L; float b1;  
        
        b1 = LegAngles.beta - (PI - LegAngles.alpha); // The part of beta that matters. 
        
        L = sin(b1) * TIBIA_L + COXA_L + sin(LegAngles.alpha) * FEMUR_L; // the extension length. 
        
        footpos_out[0] = L * cos(LEG_ANGLES[LegNum] + LegAngles.gamma); 
        footpos_out[1] = L * sin(LEG_ANGLES[LegNum] + LegAngles.gamma); 
        footpos_out[2] = -cos(LegAngles.alpha) - TIBIA_L*cos(b1) ;
         
         
    
        return footpos_out; 
}        

void  ArthropodSolver::YawXform(float invec[], float outvec[], float angle) {
    outvec[0] = invec[0]*cos(angle)- invec[1]*sin(angle); 
    outvec[1] = invec[0] * sin(angle) + outvec[1] * cos(angle);
    outvec[2] = invec[2];        
         
         
         
}
     
void  ArthropodSolver::PitchXform(float invec[], float outvec[], float angle){
    outvec[1] = invec[1]*cos(angle)- invec[2]*sin(angle); 
    outvec[2] = invec[1] * sin(angle) + outvec[2] * cos(angle);
    outvec[0] = invec[0]; 
     
}
     
void  ArthropodSolver::RollXform(float invec[], float outvec[], float angle){
    outvec[2] = invec[2]*cos(angle)- invec[0]*sin(angle); 
    outvec[0] = invec[0] * cos(angle) + outvec[2] * sin(angle);
    outvec[1] = invec[1];  
     
}