├── CHANGELOG.rst
├── CMakeLists.txt
├── README.md
├── include
    ├── adrc
    │   ├── adrc_base.h
    │   ├── adrc_cascade.h
    │   ├── adrc_firstorder.h
    │   └── adrc_secondorder.h
    └── pid.h
├── package.xml
└── src
    ├── adrc
        ├── adrc_base.cpp
        ├── adrc_cascade.cpp
        ├── adrc_firstorder.cpp
        └── adrc_secondorder.cpp
    └── pid.cpp


/CHANGELOG.rst:
--------------------------------------------------------------------------------
1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2 | Changelog for package adrc_control
3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4 | 
5 | 1.1.0 (2020-01-07)
6 | ------------------
7 | 完成二阶adrc 串级adrc 一阶adrc 控制器的编写 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(adrc_control)
 3 | 
 4 | add_definitions(-std=c++11)
 5 | 
 6 | find_package(catkin REQUIRED COMPONENTS
 7 |   roscpp
 8 |   cmake_modules
 9 | )
10 | 
11 | catkin_package(
12 |   INCLUDE_DIRS include ${Eigen3_INCLUDE_DIRS}
13 |   LIBRARIES pid 
14 |   CATKIN_DEPENDS roscpp 
15 | )
16 | 
17 | include_directories(
18 |   include
19 |   ${catkin_INCLUDE_DIRS}
20 | )
21 | 
22 | 
23 | add_library(pid
24 |   src/pid.cpp
25 | )
26 | 
27 | add_library(adrc
28 |   src/adrc/adrc_base.cpp
29 |   src/adrc/adrc_firstorder.cpp
30 |   src/adrc/adrc_secondorder.cpp
31 |   src/adrc/adrc_cascade.cpp)
32 | 
33 | install(TARGETS pid adrc
34 |   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
35 | )


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | #adrc_control


--------------------------------------------------------------------------------
/include/adrc/adrc_base.h:
--------------------------------------------------------------------------------
 1 | #ifndef ADRC_BASE_H
 2 | #define ADRC_BASE_H
 3 | #include <math.h>
 4 | #include <iostream>
 5 | #include <float.h>
 6 | namespace adrc
 7 | {
 8 |     
 9 | int sign(double val);
10 | double Fal(const double &error, const double &alpha, const double &delta);
11 | double Fhan(const double &x1, const double &x2, const double &r, const double &h);
12 | 
13 | class Adrc_Base
14 | {
15 |     friend std::ostream &operator<<(std::ostream &os, const Adrc_Base &item);
16 | 
17 | public:
18 |     Adrc_Base();
19 |     virtual ~Adrc_Base();
20 |     
21 |     virtual void TD(const double &v0);
22 |     // virtual void ESO(const double &y) = 0;
23 |     // virtual void LESO(const double &y) = 0;
24 |     // virtual double NLSEF() = 0;
25 |     virtual double ComputeControl(const double &ref, const double &cur);
26 |     void ConstraintControl(double &u);
27 |     void SetLimit(const double &limit_up, const double &limit_down);
28 |     virtual void PrintParameters(std::ostream &os) const;
29 |     
30 |     /**************通用参数*************/
31 |     double _para_h;
32 | 
33 |     /**************TD参数*************/
34 |     double _para_td_r;
35 |     
36 | 
37 | protected:
38 |     double _state_td_v1;
39 |     double _state_td_v2;
40 |     double _limit_up;
41 |     double _limit_down;
42 | };
43 | 
44 | std::ostream &operator<<(std::ostream &os, const Adrc_Base &item);
45 | } // namespace adrc
46 | 
47 | 
48 | #endif
49 | 


--------------------------------------------------------------------------------
/include/adrc/adrc_cascade.h:
--------------------------------------------------------------------------------
 1 | #ifndef ADRC_CASCADE_H
 2 | #define ADRC_CASCADE_H
 3 | #include "adrc/adrc_base.h"
 4 | namespace adrc
 5 | {
 6 | 
 7 | class Adrc_Cascade : public Adrc_Base
 8 | {
 9 | 
10 | public:
11 |     Adrc_Cascade(/* args */);
12 |     ~Adrc_Cascade();
13 |     // virtual void TD(const double &v0);
14 |     void OuterESO(const double &y);
15 |     void OuterNLSEF(const double &y);
16 |     void InnerESO(const double &y);
17 |     void InnerNLSEF();
18 |     // virtual void LESO(const double &y) = 0;
19 |     // virtual double NLSEF() = 0;
20 |     double CascadeControl(const double &ref, const double &outer_cur, const double &inner_cur);
21 |     void PrintParameters(std::ostream &os) const;
22 |     double _para_b0;
23 | 
24 |     double _para_eso_beta1;
25 |     double _para_eso_beta2;
26 | 
27 |     double _para_outer_nlsef_k;
28 |     double _para_outer_nlsef_alpha;
29 |     double _para_outer_nlsef_delta;
30 | 
31 |     double _para_inner_nlsef_k;
32 |     double _para_inner_nlsef_alpha;
33 |     double _para_inner_nlsef_delta;
34 | 
35 | private:
36 |     /* data */
37 |     double _state_outer_eso_z1;
38 |     double _state_outer_eso_z2;
39 |     double _state_outer_u;
40 | 
41 |     double _state_inner_eso_z1;
42 |     double _state_inner_eso_z2;
43 |     double _state_inner_u;
44 | };
45 | 
46 | } // namespace adrc
47 | 
48 | #endif


--------------------------------------------------------------------------------
/include/adrc/adrc_firstorder.h:
--------------------------------------------------------------------------------
 1 | #ifndef ADRC_FIRSTORDER_H
 2 | #define ADRC_FIRSTORDER_H
 3 | #include "adrc/adrc_base.h"
 4 | namespace adrc
 5 | {
 6 | 
 7 | class Adrc_FirstOrder : public Adrc_Base
 8 | {
 9 | 
10 | public:
11 |     Adrc_FirstOrder(/* args */);
12 | 
13 |     ~Adrc_FirstOrder();
14 |     // virtual void TD(const double &v0);
15 |     void LESO(const double &y);
16 |     void LSEF(const double &ref, const double &cur);
17 |     virtual double ComputeControl(const double &ref, const double &cur);
18 | 
19 |     virtual void PrintParameters(std::ostream &os) const;
20 |     double GetLESOZ1()
21 |     {
22 |         return _state_leso_z1;
23 |     }
24 | 
25 |     double GetLESOZ2()
26 |     {
27 |         return _state_leso_z2;
28 |     }
29 | 
30 |     /**************LESO参数*************/
31 |     double _para_b0;
32 |     double _para_leso_beta1;
33 |     double _para_leso_beta2;
34 |     /**************LSEF参数*************/
35 |     double _para_lsef_k1;
36 |     double _para_lsef_k2;
37 | 
38 | private:
39 |     double _state_leso_z1;
40 |     double _state_leso_z2;
41 |     double _state_cur_u;
42 | };
43 | 
44 | } // namespace adrc
45 | 
46 | #endif


--------------------------------------------------------------------------------
/include/adrc/adrc_secondorder.h:
--------------------------------------------------------------------------------
 1 | #ifndef ADRC_SECONDORDER_H
 2 | #define ADRC_SECONDORDER_H
 3 | #include "adrc/adrc_base.h"
 4 | namespace adrc
 5 | {
 6 | 
 7 | enum sef_mode_t
 8 | {
 9 |     LSEF,               //u0 = k1*e1+k2*e2
10 |     NLSEF_1,            //u0 = k1*fal(e1,alpa1,deta)+k1*fal(e2,alpa2,deta)
11 |     NLSEF_2,            //u0 = -fhan(e1,e2,r,h1)
12 |     NLSEF_3             //u0 = -fhan(e1,c*e2,r,h1)
13 | };
14 | 
15 | enum eso_mode_t
16 | {
17 |     LESO,
18 |     NLESO
19 | };
20 | 
21 | typedef struct para_eso
22 | {
23 |     eso_mode_t mode;
24 |     double beta1;
25 |     double beta2;
26 |     double beta3;
27 |     double alpha1;
28 |     double alpha2;
29 |     double delta;
30 | } para_eso_t;
31 | 
32 | typedef struct para_sef
33 | {
34 |     sef_mode_t mode;
35 |     double k1;
36 |     double k2;
37 |     double alpha1;
38 |     double alpha2;
39 |     double delta;
40 |     double r;
41 |     double c;
42 |     double h1;
43 | } para_sef_t;
44 | 
45 | class Adrc_SecondOrder : public Adrc_Base
46 | {
47 | 
48 | public:
49 |     Adrc_SecondOrder(/* args */);
50 | 
51 |     ~Adrc_SecondOrder();
52 |     void ESO(const double &y);
53 |     void SEF();
54 |     virtual double ComputeControl(const double &ref, const double &cur);
55 | 
56 |     virtual void PrintParameters(std::ostream &os) const;
57 |     
58 |     /**************通用参数*************/
59 |     double _para_b0;
60 | 
61 |     /**************ESO参数*************/
62 |     para_eso_t _para_eso;
63 |     /**************NLSEF参数*************/
64 |     para_sef_t _para_sef;
65 | 
66 | private:
67 |     double _state_eso_z1;
68 |     double _state_eso_z2;
69 |     double _state_eso_z3;
70 | 
71 |     double _state_cur_u;
72 | };
73 | 
74 | } // namespace adrc
75 | 
76 | #endif


--------------------------------------------------------------------------------
/include/pid.h:
--------------------------------------------------------------------------------
 1 | #ifndef PID_H_
 2 | #define PID_H_
 3 | 
 4 | #include <stdint.h>
 5 | typedef enum PID_MODE {
 6 |   /* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
 7 |   PID_MODE_DERIVATIV_NONE = 0,
 8 |   /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
 9 |    * val_dot in pid_calculate() will be ignored */
10 |   PID_MODE_DERIVATIV_CALC,
11 |   /* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
12 |    * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
13 |   PID_MODE_DERIVATIV_CALC_NO_SP,
14 |   /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
15 |   PID_MODE_DERIVATIV_SET
16 | } pid_mode_t;
17 | 
18 | typedef struct
19 | {
20 |   pid_mode_t mode;
21 |   float dt_min;
22 |   float kp;
23 |   float ki;
24 |   float kd;
25 |   float integral;
26 |   float integral_limit;
27 |   float output_limit;
28 |   float error_previous;
29 |   float last_output;
30 | } PID_t;
31 | 
32 | void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
33 | int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
34 | float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
35 | void pid_reset_integral(PID_t *pid);
36 | 
37 | #endif /* PID_H_ */


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <package format="2">
 2 |   <name>adrc_control</name>
 3 |   <version>1.0.0</version>
 4 |   <description>adrc control package</description>
 5 | 
 6 |   <maintainer email="lkw_nuaa@163.com">lkw</maintainer>
 7 | 
 8 |   <author>lkw</author>
 9 | 
10 |   <license>  BSD</license>
11 | 
12 |   <buildtool_depend>catkin</buildtool_depend>
13 | 
14 |   <depend>cmake_modules</depend>
15 |   <depend>roscpp</depend>
16 | 
17 | </package>
18 | 


--------------------------------------------------------------------------------
/src/adrc/adrc_base.cpp:
--------------------------------------------------------------------------------
 1 | #include "adrc/adrc_base.h"
 2 | 
 3 | namespace adrc
 4 | {
 5 | 
 6 | int sign(double val)
 7 | {
 8 |   if (val > DBL_EPSILON)
 9 |     return 1;
10 |   else  if(val  < -DBL_EPSILON)
11 |     return -1;
12 |   else return 0;
13 | }
14 | 
15 | double Fal(const double &error, const double &alpha, const double &delta)
16 | {
17 |   if (fabsf(error) > delta)
18 |   {
19 |     return sign(error) * pow(fabsf(error), alpha);
20 |   }
21 |   else
22 |   {
23 |     return error / (powf(delta, 1.0f - alpha));
24 |   }
25 | }
26 | 
27 | double Fhan(const double &x1, const double &x2, const double &r, const double &h)
28 | {
29 |   double d = r * h * h;
30 |   double a0 = h * x2;
31 |   double y = x1 + a0;
32 |   double a1 = sqrtf(d * (d + 8.0f * fabs(y)));
33 |   double a2 = a0 + sign(y) * (a1 - d) * 0.5f;
34 |   double sy = (sign(y + d) - sign(y - d)) * 0.5f;
35 |   double a = (a0 + y - a2) * sy + a2;
36 |   double sa = (sign(a + d) - sign(a - d)) * 0.5f;
37 | 
38 |   return -r * (a / d - sign(a)) * sa - r * sign(a);
39 | }
40 | 
41 | Adrc_Base::Adrc_Base()
42 |     : _state_td_v1(0.0f), _state_td_v2(0.0f), _para_td_r(2.0f), _para_h(0.005f), _limit_up(INFINITY), _limit_down(-INFINITY)
43 | {
44 | }
45 | 
46 | Adrc_Base::~Adrc_Base()
47 | {
48 | }
49 | 
50 | void Adrc_Base::TD(const double &v0)
51 | {
52 |   double fh = Fhan(_state_td_v1 - v0, _state_td_v2, _para_td_r, _para_h);
53 |   _state_td_v1 += _para_h * _state_td_v2;
54 |   _state_td_v2 += _para_h * fh;
55 | }
56 | 
57 | double Adrc_Base::ComputeControl(const double &ref, const double &cur)
58 | {
59 |   TD(ref);
60 |   ConstraintControl(_state_td_v2);
61 |   return _state_td_v2;
62 | }
63 | 
64 | void Adrc_Base::SetLimit(const double &limit_up, const double &limit_down)
65 | {
66 |   _limit_up = fabs(limit_up);
67 |   _limit_down = -fabs(limit_down);
68 | }
69 | 
70 | void Adrc_Base::ConstraintControl(double &u)
71 | {
72 |   if (u > _limit_up)
73 |   {
74 |     u = _limit_up;
75 |   }
76 |   else if (u < _limit_down)
77 |   {
78 |     u = _limit_down;
79 |   }
80 | }
81 | void Adrc_Base::PrintParameters(std::ostream &os) const
82 | {
83 |   // os << std::endl;
84 |   os << "control limit: " << _limit_down << " - " << _limit_up << std::endl;
85 |   os << "TD: r: " << _para_td_r << " h: " << _para_h << std::endl;
86 | }
87 | 
88 | std::ostream &operator<<(std::ostream &os, const Adrc_Base &obj)
89 | {
90 |   obj.PrintParameters(os);
91 |   return os;
92 | }
93 | 
94 | } // namespace adrc
95 | 


--------------------------------------------------------------------------------
/src/adrc/adrc_cascade.cpp:
--------------------------------------------------------------------------------
 1 | #include "adrc/adrc_cascade.h"
 2 | 
 3 | namespace adrc
 4 | {
 5 | Adrc_Cascade::Adrc_Cascade()
 6 |     : Adrc_Base()
 7 |     , _state_outer_eso_z1(0.0)
 8 |     , _state_outer_eso_z2(0.0)
 9 |     , _state_outer_u(0.0)
10 |     , _state_inner_eso_z1(0.0)
11 |     , _state_inner_eso_z2(0.0)
12 |     , _state_inner_u(0.0)
13 |     , _para_b0(1.0)
14 |     , _para_eso_beta1(200.0)
15 |     , _para_eso_beta2(20000.0)
16 |     , _para_outer_nlsef_k(1.0)
17 |     , _para_outer_nlsef_alpha(0.5)
18 |     , _para_outer_nlsef_delta(0.5)
19 |     , _para_inner_nlsef_k(10.0)
20 |     , _para_inner_nlsef_alpha(0.5)
21 |     , _para_inner_nlsef_delta(0.5)
22 | {
23 | }
24 | 
25 | Adrc_Cascade::~Adrc_Cascade()
26 | {
27 | }
28 | 
29 | // void Adrc_Cascade::TD(const double &v0)
30 | // {
31 | //     //_state_td_v1 -= _para_h * _para_td_r * Fal(_state_td_v1 - v0, 0.5, _para_h);
32 | //     double fh = -_para_td_r * _para_td_r * (_state_td_v1 - v0) + 2 * _para_td_r * _state_td_v2;
33 | //     _state_td_v1 += _para_h * (_state_td_v2);
34 | //     _state_td_v2 += _para_h * fh;
35 | // }
36 | 
37 | void Adrc_Cascade::OuterESO(const double &y)
38 | {
39 |     double e = _state_outer_eso_z1 - y;
40 |     // double fe = Fal(e, 0.5, _para_h);
41 |     _state_outer_eso_z1 += _para_h * (_state_outer_eso_z2 - _para_eso_beta1 * e + _state_outer_u);
42 |     _state_outer_eso_z2 += _para_h * (-_para_eso_beta2 * e);
43 | }
44 | 
45 | void Adrc_Cascade::OuterNLSEF(const double &y)
46 | {
47 |     // double e = _state_td_v1 - _state_outer_eso_z1;
48 |     double e = _state_td_v1 - y;
49 |     // _state_outer_u = _para_outer_nlsef_k * Fal(e, _para_outer_nlsef_alpha, _para_outer_nlsef_delta);
50 |     _state_outer_u = _para_outer_nlsef_k * e;
51 |     // _state_outer_u -= _state_outer_eso_z2;
52 | }
53 | 
54 | void Adrc_Cascade::InnerESO(const double &y)
55 | {
56 |     double e = _state_inner_eso_z1 - y;
57 |     // double fe = Fal(e, 0.5, _para_h);
58 |     _state_inner_eso_z1 += _para_h * (_state_inner_eso_z2 - _para_eso_beta1 * e + _para_b0 *_state_inner_u);
59 |     _state_inner_eso_z2 += _para_h * (-_para_eso_beta2 * e);
60 | }
61 | 
62 | void Adrc_Cascade::InnerNLSEF()
63 | {
64 |     double e = _state_outer_u - _state_inner_eso_z1;
65 |     // _state_inner_u = _para_inner_nlsef_k * Fal(e, _para_inner_nlsef_alpha, _para_inner_nlsef_delta);
66 |     _state_inner_u = _para_inner_nlsef_k * e;
67 |     _state_inner_u -= _state_inner_eso_z2;
68 |     _state_inner_u /= _para_b0;
69 | }
70 | 
71 | double Adrc_Cascade::CascadeControl(const double &ref, const double &outer_cur, const double &inner_cur)
72 | {
73 |     TD(ref);
74 |     // OuterESO(outer_cur);
75 |     OuterNLSEF(outer_cur);
76 |     InnerESO(inner_cur);
77 |     InnerNLSEF();
78 | 
79 | 
80 |     
81 |     ConstraintControl(_state_inner_u);
82 |     return _state_inner_u;
83 | }
84 | void Adrc_Cascade::PrintParameters(std::ostream &os) const
85 | {
86 |     Adrc_Base::PrintParameters(os);
87 |     os << "b0: " << _para_b0 << " ";
88 |     os << "eso beta1: " << _para_eso_beta1 << " beta2: " << _para_eso_beta2 << std::endl;
89 |     os << "inner_nlsef k: " << _para_outer_nlsef_k << " alpha: " << _para_outer_nlsef_alpha << " delta： " << _para_outer_nlsef_delta << std::endl;
90 |     os << "inner_nlsef k: " << _para_inner_nlsef_k << " alpha: " << _para_inner_nlsef_alpha << " delta： " << _para_inner_nlsef_delta << std::endl;
91 | }
92 | } // namespace adrc
93 | 


--------------------------------------------------------------------------------
/src/adrc/adrc_firstorder.cpp:
--------------------------------------------------------------------------------
 1 | #include "adrc/adrc_firstorder.h"
 2 | 
 3 | namespace adrc
 4 | {
 5 | 
 6 | Adrc_FirstOrder::Adrc_FirstOrder()
 7 |     : Adrc_Base()
 8 |     , _state_leso_z1(0.0)
 9 |     , _state_leso_z2(0.0)
10 |     , _state_cur_u(0.0)
11 |     , _para_b0(1.0)
12 |     , _para_leso_beta1(200.0)
13 |     , _para_leso_beta2(13333.0)
14 |     , _para_lsef_k1(10.0)
15 |     , _para_lsef_k2(10.0)
16 | {
17 | }
18 | 
19 | Adrc_FirstOrder::~Adrc_FirstOrder()
20 | {
21 | }
22 | // void Adrc_FirstOrder::TD(const double &v0)
23 | // {
24 | //     //_state_td_v1 -= _para_h * _para_td_r * Fal(_state_td_v1 - v0, 0.5, _para_h);
25 | //     double fh = -_para_td_r * _para_td_r * (_state_td_v1 - v0) + 2 * _para_td_r * _state_td_v2;
26 | //     _state_td_v1 += _para_h * (_state_td_v2);
27 | //     _state_td_v2 += _para_h * fh;
28 | //     _state_td_v1 = v0;
29 | // }
30 | void Adrc_FirstOrder::LESO(const double &y)
31 | {
32 |     double e = _state_leso_z1 - y;
33 |     _state_leso_z1 += _para_h * (_state_leso_z2 - _para_leso_beta1 * e + _para_b0 * _state_cur_u);
34 |     _state_leso_z2 += _para_h * (-_para_leso_beta2 * e);
35 | }
36 | void Adrc_FirstOrder::LSEF(const double &ref, const double &cur)
37 | {
38 |     double e = ref - _state_leso_z1;
39 |     // double e = _state_td_v1 - _state_leso_z1;
40 |     // double e1 = _state_td_v2;
41 |     _state_cur_u = _para_lsef_k1 * e;// + _para_lsef_k2 * e1;
42 |     _state_cur_u -= _state_leso_z2;
43 |     _state_cur_u /= _para_b0;
44 | }
45 | double Adrc_FirstOrder::ComputeControl(const double &ref, const double &cur)
46 | {
47 |     //TD(ref-cur);
48 |     LESO(cur);
49 |     LSEF(ref, cur);
50 |     ConstraintControl(_state_cur_u);
51 |     return _state_cur_u;
52 | }
53 | 
54 | void Adrc_FirstOrder::PrintParameters(std::ostream &os) const
55 | {
56 |     Adrc_Base::PrintParameters(os);
57 |     os << "LESO: beta1: " << _para_leso_beta1 << " beta2: " << _para_leso_beta2 << std::endl;
58 |     os << "LSEF: k1: " << _para_lsef_k1 << " k2: " << _para_lsef_k2 << " b0: " << _para_b0 << std::endl;
59 |     os<<std::endl;
60 | }
61 | } // namespace adrc
62 | 


--------------------------------------------------------------------------------
/src/adrc/adrc_secondorder.cpp:
--------------------------------------------------------------------------------
  1 | #include "adrc/adrc_secondorder.h"
  2 | namespace adrc
  3 | {
  4 |     Adrc_SecondOrder::Adrc_SecondOrder()
  5 |         : Adrc_Base()
  6 |         , _state_eso_z1(0.0)
  7 |         , _state_eso_z2(0.0)
  8 |         , _state_eso_z3(0.0)
  9 |         , _state_cur_u(0.0)
 10 |         , _para_b0(1.0)
 11 |         , _para_eso({mode : NLESO, beta1 : 200.0, beta2 : 1768.0, beta3 : 13420.0, alpha1 : 0.5, alpha2 : 0.25, delta : 0.05})
 12 |         , _para_sef({mode : NLSEF_1, k1 : 10.0, k2 : 10.0, alpha1 : 0.75, alpha2 : 1.25, delta : 0.5, r : 20000.0, c : 0.5, h1 : 0.025})
 13 |     {
 14 | 
 15 |     }
 16 | 
 17 |     Adrc_SecondOrder::~Adrc_SecondOrder()
 18 |     {}
 19 | 
 20 |     void Adrc_SecondOrder::ESO(const double &y)
 21 |     {
 22 |         double e = _state_eso_z1 - y;
 23 |         _state_eso_z1 += _para_h * (_state_eso_z2 - _para_eso.beta1 * e);
 24 |         
 25 |         switch (_para_eso.mode)
 26 |         {
 27 |         case LESO:
 28 |             {
 29 |                 _state_eso_z2 += _para_h * (_state_eso_z3 - _para_eso.beta2 * e + _para_b0 * _state_cur_u);
 30 |                 _state_eso_z3 += _para_h * (-_para_eso.beta3 * e);
 31 |             }
 32 |             break;
 33 |         case NLESO:
 34 |             {
 35 |                 double fe = Fal(e, _para_eso.alpha1, _para_eso.delta);
 36 |                 double fe1 = Fal(e, _para_eso.alpha2, _para_eso.delta);
 37 |                 _state_eso_z2 += _para_h * (_state_eso_z3 - _para_eso.beta2 * fe + _para_b0 * _state_cur_u);
 38 |                 _state_eso_z3 += _para_h * (-_para_eso.beta3 * fe1);
 39 |             }
 40 |             break;
 41 |         default:
 42 |             break;
 43 |         }
 44 |     }
 45 | 
 46 |     void Adrc_SecondOrder::SEF()
 47 |     {
 48 |         double e1 = _state_td_v1 - _state_eso_z1;
 49 |         double e2 = _state_td_v2 - _state_eso_z2;
 50 |         
 51 |         switch (_para_sef.mode)
 52 |         {
 53 |         case LSEF:
 54 |             _state_cur_u = _para_sef.k1 * e1 + _para_sef.k2 * e2;
 55 |             break;
 56 |         case NLSEF_1:
 57 |             _state_cur_u = _para_sef.k1*Fal(e1,_para_sef.alpha1,_para_sef.delta)
 58 |                          + _para_sef.k2*Fal(e2,_para_sef.alpha2,_para_sef.delta);
 59 |             break;
 60 |         case NLSEF_2:
 61 |             _state_cur_u = -Fhan(e1, e2, _para_sef.r, _para_sef.h1);
 62 |             break;
 63 |         case NLSEF_3:
 64 |             _state_cur_u = -Fhan(e1, _para_sef.c * e2, _para_sef.r, _para_sef.h1);
 65 |             break;
 66 | 
 67 |         default:
 68 |             break;
 69 |         }
 70 | 
 71 |         _state_cur_u -= _state_eso_z3;
 72 |         _state_cur_u /= _para_b0;
 73 |     }
 74 | 
 75 |     double Adrc_SecondOrder::ComputeControl(const double &ref, const double &cur)
 76 |     {
 77 |         TD(ref);
 78 |         ESO(cur);
 79 |         SEF();
 80 |         ConstraintControl(_state_cur_u);
 81 |         return _state_cur_u;
 82 |     }
 83 | 
 84 |     void Adrc_SecondOrder::PrintParameters(std::ostream &os) const
 85 |     {
 86 |         Adrc_Base::PrintParameters(os);
 87 |         os << "ESO: "<< "beta1: " << _para_eso.beta1 << " beta2: " << _para_eso.beta2 << " beta3: " << _para_eso.beta3 << std::endl;
 88 | 
 89 |         if(_para_eso.mode == NLESO)
 90 |         {
 91 |             os << "alpha1: "<<_para_eso.alpha1<<" alpha2: "<<_para_eso.alpha2<<" delta"<<_para_eso.delta << std::endl;
 92 |         }
 93 |         os << "SEF: b0: " <<_para_b0<< std::endl;
 94 | 
 95 |         switch (_para_sef.mode)
 96 |         {
 97 |         case LSEF:
 98 |             os << "k1: " << _para_sef.k1 <<" k2:"<<_para_sef.k2<< std::endl;
 99 |             break;
100 |         case NLSEF_1:
101 |             os << "k1: " << _para_sef.k1 << " k2:" << _para_sef.k2 << std::endl;
102 |             os << "alpha1: " << _para_sef.alpha1 << " alpha2: " << _para_sef.alpha2 << " delta" << _para_sef.delta << std::endl;
103 |             break;
104 |         case NLSEF_2:
105 |             os << "r1: " << _para_sef.r << " h1: " << _para_sef.h1 << std::endl;
106 |             break;
107 |         case NLSEF_3:
108 |             os << "r1: " << _para_sef.r << " c: " << _para_sef.c << " h1: " << _para_sef.h1 << std::endl;
109 |             break;
110 | 
111 |         default:
112 |             break;
113 |         }
114 |     }
115 | } // namespace adrc
116 | 


--------------------------------------------------------------------------------
/src/pid.cpp:
--------------------------------------------------------------------------------
  1 | #include <math.h>
  2 | #include <algorithm>
  3 | #include "pid.h"
  4 | using namespace std;
  5 | #define SIGMA 0.000001f
  6 | 
  7 | void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
  8 | {
  9 |   pid->mode = mode;
 10 |   pid->dt_min = dt_min;
 11 |   pid->kp = 0.0f;
 12 |   pid->ki = 0.0f;
 13 |   pid->kd = 0.0f;
 14 |   pid->integral = 0.0f;
 15 |   pid->integral_limit = 0.0f;
 16 |   pid->output_limit = 0.0f;
 17 |   pid->error_previous = 0.0f;
 18 |   pid->last_output = 0.0f;
 19 | }
 20 | 
 21 | int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
 22 | {
 23 |   int ret = 0;
 24 | 
 25 |   if (isfinite(kp))
 26 |   {
 27 |     pid->kp = kp;
 28 |   }
 29 |   else
 30 |   {
 31 |     ret = 1;
 32 |   }
 33 | 
 34 |   if (isfinite(ki))
 35 |   {
 36 |     pid->ki = ki;
 37 |   }
 38 |   else
 39 |   {
 40 |     ret = 1;
 41 |   }
 42 | 
 43 |   if (isfinite(kd))
 44 |   {
 45 |     pid->kd = kd;
 46 |   }
 47 |   else
 48 |   {
 49 |     ret = 1;
 50 |   }
 51 | 
 52 |   if (isfinite(integral_limit))
 53 |   {
 54 |     pid->integral_limit = integral_limit;
 55 |   }
 56 |   else
 57 |   {
 58 |     ret = 1;
 59 |   }
 60 | 
 61 |   if (isfinite(output_limit))
 62 |   {
 63 |     pid->output_limit = output_limit;
 64 |   }
 65 |   else
 66 |   {
 67 |     ret = 1;
 68 |   }
 69 | 
 70 |   return ret;
 71 | }
 72 | 
 73 | float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
 74 | {
 75 |   if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt))
 76 |   {
 77 |     return pid->last_output;
 78 |   }
 79 | 
 80 |   float i, d;
 81 | 
 82 |   /* current error value */
 83 |   float error = sp - val;
 84 | 
 85 |   /* current error derivative */
 86 |   if (pid->mode == PID_MODE_DERIVATIV_CALC)
 87 |   {
 88 |     d = (error - pid->error_previous) / max(dt, pid->dt_min);
 89 |     pid->error_previous = error;
 90 |   }
 91 |   else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP)
 92 |   {
 93 |     d = (-val - pid->error_previous) / max(dt, pid->dt_min);
 94 |     pid->error_previous = -val;
 95 |   }
 96 |   else if (pid->mode == PID_MODE_DERIVATIV_SET)
 97 |   {
 98 |     d = -val_dot;
 99 |   }
100 |   else
101 |   {
102 |     d = 0.0f;
103 |   }
104 | 
105 |   if (!isfinite(d))
106 |   {
107 |     d = 0.0f;
108 |   }
109 | 
110 |   /* calculate PD output */
111 |   float output = (error * pid->kp) + (d * pid->kd);
112 | 
113 |   if (pid->ki > SIGMA)
114 |   {
115 |     // Calculate the error integral and check for saturation
116 |     i = pid->integral + (error * dt);
117 | 
118 |     /* check for saturation */
119 |     if (isfinite(i))
120 |     {
121 |       if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
122 |           fabsf(i) <= pid->integral_limit)
123 |       {
124 |         /* not saturated, use new integral value */
125 |         pid->integral = i;
126 |       }
127 |     }
128 | 
129 |     /* add I component to output */
130 |     output += pid->integral * pid->ki;
131 |   }
132 | 
133 |   /* limit output */
134 |   if (isfinite(output))
135 |   {
136 |     if (pid->output_limit > SIGMA)
137 |     {
138 |       if (output > pid->output_limit)
139 |       {
140 |         output = pid->output_limit;
141 |       }
142 |       else if (output < -pid->output_limit)
143 |       {
144 |         output = -pid->output_limit;
145 |       }
146 |     }
147 | 
148 |     pid->last_output = output;
149 |   }
150 | 
151 |   return pid->last_output;
152 | }
153 | 
154 | void pid_reset_integral(PID_t *pid)
155 | {
156 |   pid->integral = 0.0f;
157 | }


--------------------------------------------------------------------------------