Diff: Stabilizer.cpp

Revision:

0:59d3683c6d99

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Stabilizer.cpp	Wed Dec 23 11:17:22 2015 +0000
@@ -0,0 +1,112 @@
+#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;
+}
+