File content as of revision 0:59d3683c6d99:

#include "Stabilizer.h"
#include "mbed.h"
//#include "dsp.h"


Stabilizer::Stabilizer()
{
    integral=0;
    derivative=0;
    error=0;
    previous_error=0;
    New_Height = 0;
    zeta_set=0;

    Offset_Y=0;
    Offset_Z=0;
    
    // communication to eeprom
    // get link hip, link knee
    float l_hip=1.0f;
    float l_knee=1.0f;
    float r_hip=1.0f;
    float r_knee=1.0f;
    
    Leg_Left = new Kinematic(l_hip,l_knee);
    Leg_Right = new Kinematic(r_hip,r_knee);
    
}

/*
Stabilizer::Stabilizer(float Body_Lenght)
{
    //this->Body_Lenght = Body_Lenght;

    integral=0;
    derivative=0;
    error=0;
    previous_error=0;
    New_Height = 0;
    zeta_set=0;
}
*/
Stabilizer::Stabilizer(float init_y, float init_z)
{
    this->Offset_Y = init_y;
    this->Offset_Z = init_z;

    //this->zeta_set = zeta_set;

    integral=0;
    derivative=0;
    error=0;
    previous_error=0;
    New_Height = 0;
    
    // communication to eeprom
    // get link hip, link knee
    float l_hip=1.0f;
    float l_knee=1.0f;
    float r_hip=1.0f;
    float r_knee=1.0f;
    
    Leg_Left = new Kinematic(l_hip,l_knee);
    Leg_Right = new Kinematic(r_hip,r_knee);
}

void Stabilizer::set_Body_Lenght(float Body_Lenght)
{
    this->Body_Lenght = Body_Lenght;
}

void Stabilizer::set_current_zeta(float zeta)
{
    current_zeta = zeta;
}

void Stabilizer::set_New_Height(float height)
{
    New_Height = height+delta_h;
}

void Stabilizer::set_zeta_set(float zeta_set)
{
    this->zeta_set = zeta_set;
}

/*
void Stabilizer::ZetaErrorCalculation()
{
    error = zeta_set-current_zeta;
    printf("error : %f\n",error);
}
*/

float Stabilizer::PID()
{
    error = zeta_set-current_zeta;
    //PID control
    integral += error;
    derivative = error - previous_error;
    
    output = (KP*error) + (KI*integral) + (KD*derivative);
    
    previous_error = error;
    

    //output = Body_Lenght*(sin(output*0.174f)); //delta h
    delta_h = output;

    return delta_h;
}