File content as of revision 3:fd037c89ba5f:

#include "Kinematic.h"
#include "math.h"

#define Kinematic_Debug

#define PI 3.1415926

Kinematic::Kinematic(float Link_Hip, float Link_Knee)
{
    this->Link_Hip = Link_Hip;
    this->Link_Knee = Link_Knee;
}

Kinematic::Kinematic(char text, float Link_Hip, float Link_Knee, float Hip, float Knee)
{
    if(text == 'Z' || text == 'z') {
        this->Link_Hip = Link_Hip;
        this->Link_Knee = Link_Knee;
        Zeta_Hip = Hip;
        Zeta_Knee = Knee;
        ForwardKinematicCalculation();
#ifdef Kinematic_Debug
        test.printf("Forward Kinematic\n");
#endif
    }

    if(text == 'P' || text == 'p') {
        this->Link_Hip = Link_Hip;
        this->Link_Knee = Link_Knee;
        Position_Y = Hip;
        Position_Z = Knee;
        InverseKinematicCalculation();
#ifdef Kinematic_Debug
        test.printf("Inverse Kinematic\n");
#endif
    }
}

void Kinematic::set_Link_Hip(float Link_Hip)
{
    this->Link_Hip = Link_Hip;
}

void Kinematic::set_Link_Knee(float Link_Knee)
{
    this->Link_Knee = Link_Knee;
}

void Kinematic::set_Zeta_Hip(float Zeta_Hip)
{
    this->Zeta_Hip = Zeta_Hip;
}

void Kinematic::set_Zeta_Knee(float Zeta_Knee)
{
    this->Zeta_Knee = Zeta_Knee;
}

void Kinematic::set_Position_Y(float Position_Y)
{
    this->Position_Y = Position_Y;
}

void Kinematic::set_Position_Z(float Position_Z)
{
    this->Position_Z = Position_Z;
}

void Kinematic::set_zeta_knee_range(float zeta1,float zeta2)
{
    zeta_knee_range_max = zeta2;
    zeta_knee_range_min = zeta1;
}

void Kinematic::set_zeta_hip_range(float zeta1,float zeta2)
{
    zeta_hip_range_max = zeta2;
    zeta_hip_range_min = zeta1;
}

void Kinematic::set_offset_YZ(float offset_y, float offset_z)
{
    this->offset_y = offset_y;
    this->offset_z = offset_z;
}

void Kinematic::SumPositionWithOffset()
{
    Position_Y += offset_y;
    Position_Z += offset_z;
}


void Kinematic::ForwardKinematicCalculation()
{
    Position_Y = Link_Hip*sin(Link_Knee)+Link_Knee*sin(Link_Knee+Zeta_Knee); //zeta1-zeta2
    Position_Z = Link_Hip*cos(Link_Knee)+Link_Knee*cos(Link_Knee+Zeta_Knee);
}

void Kinematic::InverseKinematicCalculation()
{
    // Begin by working out L
    float L;
    L = (Position_Y*Position_Y) + (Position_Z*Position_Z);
    L = sqrt(L);
#ifdef Kinematic_Debug
    test.printf("L : %f\n",L);
#endif

// Work out the Knee angle
    //test.printf("Link Hip : %f, Link Knee : %f\n",Link_Hip,Link_Knee);
    float Knee;
    Knee = pow(Link_Hip,2)+pow(Link_Knee,2)-pow(L,2);
    Knee = Knee / (2*Link_Hip*Link_Knee);
    Knee = acos(Knee);
    Knee = Knee * 180 / PI;
    Zeta_Knee = Knee;
#ifdef Kinematic_Debug
    test.printf("Zeta Knee : %f\n",Zeta_Knee);
#endif

// Work out Alpha
    float Alpha = (Link_Hip*Link_Hip)+(L*L)-(Link_Knee*Link_Knee);
    Alpha = Alpha / (2*Link_Hip*L);
    Alpha = acos(Alpha);
#ifdef Kinematic_Debug
    test.printf("Alpha : %f\n",Alpha);
#endif


// Work out Beta
    float Beta = (float) Position_Y/(float) Position_Z;
    Beta = atan(Beta);
#ifdef Kinematic_Debug
    test.printf("Beta : %f\n",Beta);
#endif

// FinallZ work out the Hip angle
    float Hip = PI - Alpha - Beta;
    Hip = Hip * 180 / PI;
    Zeta_Hip = Hip;
#ifdef Kinematic_Debug
    test.printf("Zeta Hip : %f\n",Zeta_Hip);
#endif

#ifdef Kinematic_Debug
    test.printf("Y = %f, Z = %.2f\n",Position_Y,Position_Z);
#endif
    //test.test.test.test.test.printf("Height in Stabilizer : %f\n",height);
}

void Kinematic::print()
{
    if(!(Zeta_Hip > zeta_hip_range_max || Zeta_Hip < zeta_hip_range_min)) test.printf("Z-Hip : Out of Range, ");
    else test.printf("Z-Hip : %f, ",Zeta_Hip);
    if(!(Zeta_Knee > zeta_knee_range_max || Zeta_Knee < zeta_knee_range_min)) test.printf("Z-Knee : Out of Range, ");
    else test.printf("Z-Knee : %f, ",Zeta_Knee);
    test.printf("Y : %f, Z : %f\n",Position_Y,Position_Z);
}