control dogwheelchair
Dependencies: InverseLeg
Dependents: MPU9250AHRS-PGear_Stabilizer
Diff: Stabilizer.cpp
- Revision:
- 0:59d3683c6d99
diff -r 000000000000 -r 59d3683c6d99 Stabilizer.cpp --- /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; +} +