BE@R lab / Stabilizer

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?

UserRevisionLine numberNew 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