control dogwheelchair
Dependencies: InverseLeg
Dependents: MPU9250AHRS-PGear_Stabilizer
Diff: Stabilizer.h
- Revision:
- 0:59d3683c6d99
- Child:
- 2:0ba602e6fc75
diff -r 000000000000 -r 59d3683c6d99 Stabilizer.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Stabilizer.h Wed Dec 23 11:17:22 2015 +0000 @@ -0,0 +1,76 @@ +#include "Kinematic.h" + +class Stabilizer +{ +private: + //PID + float zeta_set; + float error; + + float integral; + float derivative; + + float previous_error; + float current_zeta; + float Body_Lenght; + + float output; + + float delta_h; + float New_Height; + + float KP; + float KI; + float KD; + + float Offset_Y,Offset_Z; + + Kinematic *Leg_Left, *Leg_Right; + //Leg_Right; + +public: + Stabilizer(); + //Stabilizer(float); //Body_Lenght + Stabilizer(float,float); //init offset axis Y & Z + + void Init_Stabilizer(float,float); + + void set_KP(float); + void set_KI(float); + void set_KD(float); + + float PID(); + //void ZetaErrorCalculation(); + + void set_current_zeta(float); + void set_Body_Lenght(float); + void set_New_Height(float); + void set_zeta_set(float); + + void set_Offset_Y(float); + void set_Offset_Z(float); + + float get_current_zeta() { + return current_zeta; + } + float get_set_zeta() { + return zeta_set; + } + float get_Body_Lenght() { + return Body_Lenght; + } + float get_delta_h() { + return delta_h; + } + float get_New_Height() { + return New_Height; + } + + float get_Offset_Y() { + return Offset_Y; + } + float get_Offset_Z() { + return Offset_Z; + } + +}; \ No newline at end of file