control dogwheelchair
Dependencies: InverseLeg
Dependents: MPU9250AHRS-PGear_Stabilizer
Stabilizer.h@3:0ced46e2059e, 2016-01-23 (annotated)
- Committer:
- soulx
- Date:
- Sat Jan 23 17:08:07 2016 +0000
- Revision:
- 3:0ced46e2059e
- Parent:
- 2:0ba602e6fc75
-
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
soulx | 2:0ba602e6fc75 | 1 | #ifndef _STABILIZER_H_ |
soulx | 2:0ba602e6fc75 | 2 | #define _STABILIZER_H_ |
soulx | 2:0ba602e6fc75 | 3 | |
soulx | 0:59d3683c6d99 | 4 | #include "Kinematic.h" |
soulx | 0:59d3683c6d99 | 5 | |
soulx | 0:59d3683c6d99 | 6 | class Stabilizer |
soulx | 0:59d3683c6d99 | 7 | { |
soulx | 0:59d3683c6d99 | 8 | private: |
soulx | 0:59d3683c6d99 | 9 | //PID |
soulx | 0:59d3683c6d99 | 10 | float zeta_set; |
soulx | 0:59d3683c6d99 | 11 | float error; |
soulx | 0:59d3683c6d99 | 12 | |
soulx | 0:59d3683c6d99 | 13 | float integral; |
soulx | 0:59d3683c6d99 | 14 | float derivative; |
soulx | 0:59d3683c6d99 | 15 | |
soulx | 0:59d3683c6d99 | 16 | float previous_error; |
soulx | 0:59d3683c6d99 | 17 | float current_zeta; |
soulx | 0:59d3683c6d99 | 18 | float Body_Lenght; |
soulx | 0:59d3683c6d99 | 19 | |
soulx | 0:59d3683c6d99 | 20 | float output; |
soulx | 0:59d3683c6d99 | 21 | |
soulx | 0:59d3683c6d99 | 22 | float delta_h; |
soulx | 0:59d3683c6d99 | 23 | float New_Height; |
soulx | 0:59d3683c6d99 | 24 | |
soulx | 0:59d3683c6d99 | 25 | float KP; |
soulx | 0:59d3683c6d99 | 26 | float KI; |
soulx | 0:59d3683c6d99 | 27 | float KD; |
soulx | 0:59d3683c6d99 | 28 | |
soulx | 0:59d3683c6d99 | 29 | float Offset_Y,Offset_Z; |
soulx | 0:59d3683c6d99 | 30 | |
soulx | 0:59d3683c6d99 | 31 | Kinematic *Leg_Left, *Leg_Right; |
soulx | 0:59d3683c6d99 | 32 | //Leg_Right; |
soulx | 0:59d3683c6d99 | 33 | |
soulx | 0:59d3683c6d99 | 34 | public: |
soulx | 0:59d3683c6d99 | 35 | Stabilizer(); |
soulx | 0:59d3683c6d99 | 36 | //Stabilizer(float); //Body_Lenght |
soulx | 0:59d3683c6d99 | 37 | Stabilizer(float,float); //init offset axis Y & Z |
soulx | 0:59d3683c6d99 | 38 | |
soulx | 0:59d3683c6d99 | 39 | void Init_Stabilizer(float,float); |
soulx | 0:59d3683c6d99 | 40 | |
soulx | 0:59d3683c6d99 | 41 | void set_KP(float); |
soulx | 0:59d3683c6d99 | 42 | void set_KI(float); |
soulx | 0:59d3683c6d99 | 43 | void set_KD(float); |
soulx | 0:59d3683c6d99 | 44 | |
soulx | 0:59d3683c6d99 | 45 | float PID(); |
soulx | 0:59d3683c6d99 | 46 | //void ZetaErrorCalculation(); |
soulx | 0:59d3683c6d99 | 47 | |
soulx | 0:59d3683c6d99 | 48 | void set_current_zeta(float); |
soulx | 0:59d3683c6d99 | 49 | void set_Body_Lenght(float); |
soulx | 0:59d3683c6d99 | 50 | void set_New_Height(float); |
soulx | 0:59d3683c6d99 | 51 | void set_zeta_set(float); |
soulx | 0:59d3683c6d99 | 52 | |
soulx | 0:59d3683c6d99 | 53 | void set_Offset_Y(float); |
soulx | 0:59d3683c6d99 | 54 | void set_Offset_Z(float); |
soulx | 0:59d3683c6d99 | 55 | |
soulx | 0:59d3683c6d99 | 56 | float get_current_zeta() { |
soulx | 0:59d3683c6d99 | 57 | return current_zeta; |
soulx | 0:59d3683c6d99 | 58 | } |
soulx | 0:59d3683c6d99 | 59 | float get_set_zeta() { |
soulx | 0:59d3683c6d99 | 60 | return zeta_set; |
soulx | 0:59d3683c6d99 | 61 | } |
soulx | 0:59d3683c6d99 | 62 | float get_Body_Lenght() { |
soulx | 0:59d3683c6d99 | 63 | return Body_Lenght; |
soulx | 0:59d3683c6d99 | 64 | } |
soulx | 0:59d3683c6d99 | 65 | float get_delta_h() { |
soulx | 0:59d3683c6d99 | 66 | return delta_h; |
soulx | 0:59d3683c6d99 | 67 | } |
soulx | 0:59d3683c6d99 | 68 | float get_New_Height() { |
soulx | 0:59d3683c6d99 | 69 | return New_Height; |
soulx | 0:59d3683c6d99 | 70 | } |
soulx | 0:59d3683c6d99 | 71 | |
soulx | 0:59d3683c6d99 | 72 | float get_Offset_Y() { |
soulx | 0:59d3683c6d99 | 73 | return Offset_Y; |
soulx | 0:59d3683c6d99 | 74 | } |
soulx | 0:59d3683c6d99 | 75 | float get_Offset_Z() { |
soulx | 0:59d3683c6d99 | 76 | return Offset_Z; |
soulx | 0:59d3683c6d99 | 77 | } |
soulx | 0:59d3683c6d99 | 78 | |
soulx | 2:0ba602e6fc75 | 79 | }; |
soulx | 2:0ba602e6fc75 | 80 | |
soulx | 2:0ba602e6fc75 | 81 | #endif |