A new type of anti-slip controller based on TS fuzzy models
ANTI_SLIP_FUZZY_CONTROL.h@0:bfcd2371f3dc, 2017-02-23 (annotated)
- Committer:
- benson516
- Date:
- Thu Feb 23 11:12:35 2017 +0000
- Revision:
- 0:bfcd2371f3dc
- Child:
- 1:773d8ae11c1a
Test succeed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
benson516 | 0:bfcd2371f3dc | 1 | #ifndef ANTI_SLIP_FUZZY_CONTROL_H |
benson516 | 0:bfcd2371f3dc | 2 | #define ANTI_SLIP_FUZZY_CONTROL_H |
benson516 | 0:bfcd2371f3dc | 3 | // |
benson516 | 0:bfcd2371f3dc | 4 | #include <vector> |
benson516 | 0:bfcd2371f3dc | 5 | #include "FILTER_LIB.h" |
benson516 | 0:bfcd2371f3dc | 6 | |
benson516 | 0:bfcd2371f3dc | 7 | using std::vector; |
benson516 | 0:bfcd2371f3dc | 8 | |
benson516 | 0:bfcd2371f3dc | 9 | //------------------------------------------// |
benson516 | 0:bfcd2371f3dc | 10 | // The template for building a library |
benson516 | 0:bfcd2371f3dc | 11 | // for control system apllication |
benson516 | 0:bfcd2371f3dc | 12 | //------------------------------------------// |
benson516 | 0:bfcd2371f3dc | 13 | |
benson516 | 0:bfcd2371f3dc | 14 | // The plant is a (p, n, q) system |
benson516 | 0:bfcd2371f3dc | 15 | // Dimensions: |
benson516 | 0:bfcd2371f3dc | 16 | // |
benson516 | 0:bfcd2371f3dc | 17 | // Inputs, u | States, x | outputs, y |
benson516 | 0:bfcd2371f3dc | 18 | // p --> n --> q |
benson516 | 0:bfcd2371f3dc | 19 | // |
benson516 | 0:bfcd2371f3dc | 20 | |
benson516 | 0:bfcd2371f3dc | 21 | class ANTI_SLIP_FUZZY_CONTROL{ |
benson516 | 0:bfcd2371f3dc | 22 | public: |
benson516 | 0:bfcd2371f3dc | 23 | // Dimensions |
benson516 | 0:bfcd2371f3dc | 24 | size_t n; // Number of states |
benson516 | 0:bfcd2371f3dc | 25 | size_t p; // Number of inputs of the plant |
benson516 | 0:bfcd2371f3dc | 26 | size_t q; // Number of outputs of the plant, no use in full state feed back case |
benson516 | 0:bfcd2371f3dc | 27 | // |
benson516 | 0:bfcd2371f3dc | 28 | size_t m_vertex; // Number of vertex systems |
benson516 | 0:bfcd2371f3dc | 29 | |
benson516 | 0:bfcd2371f3dc | 30 | float Ts; // Sampling time |
benson516 | 0:bfcd2371f3dc | 31 | |
benson516 | 0:bfcd2371f3dc | 32 | // System parameters |
benson516 | 0:bfcd2371f3dc | 33 | vector<vector<float> > E_out; // System output matrix |
benson516 | 0:bfcd2371f3dc | 34 | // Controller parameters |
benson516 | 0:bfcd2371f3dc | 35 | // Parameters of vertex controller (k = 1,...,m_vertex) |
benson516 | 0:bfcd2371f3dc | 36 | vector<vector<vector<float> > > ver_K_matrix; // The list of gain matrices for each vertex system, full gain matrix of full state feedback with integral action |
benson516 | 0:bfcd2371f3dc | 37 | |
benson516 | 0:bfcd2371f3dc | 38 | |
benson516 | 0:bfcd2371f3dc | 39 | // States |
benson516 | 0:bfcd2371f3dc | 40 | // Input signal --- |
benson516 | 0:bfcd2371f3dc | 41 | vector<float> states; // States |
benson516 | 0:bfcd2371f3dc | 42 | vector<float> command; // r, commands |
benson516 | 0:bfcd2371f3dc | 43 | // Output signal --- |
benson516 | 0:bfcd2371f3dc | 44 | vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller |
benson516 | 0:bfcd2371f3dc | 45 | |
benson516 | 0:bfcd2371f3dc | 46 | // Internal states --- |
benson516 | 0:bfcd2371f3dc | 47 | vector<vector<float> > ver_u; // output of the each vertex controller |
benson516 | 0:bfcd2371f3dc | 48 | vector<float> sys_outputs; // The output of the plant, "y", the input of the controller |
benson516 | 0:bfcd2371f3dc | 49 | // Integral state |
benson516 | 0:bfcd2371f3dc | 50 | vector<float> state_int; // x_i |
benson516 | 0:bfcd2371f3dc | 51 | // Total states, [states; state_int] |
benson516 | 0:bfcd2371f3dc | 52 | vector<float> state_total; |
benson516 | 0:bfcd2371f3dc | 53 | |
benson516 | 0:bfcd2371f3dc | 54 | // |
benson516 | 0:bfcd2371f3dc | 55 | // The composition ratio of each vertex system |
benson516 | 0:bfcd2371f3dc | 56 | vector<float> ver_ratio; // ver_ratio \in R^m_vertex, its values are in [0, 1] |
benson516 | 0:bfcd2371f3dc | 57 | |
benson516 | 0:bfcd2371f3dc | 58 | |
benson516 | 0:bfcd2371f3dc | 59 | |
benson516 | 0:bfcd2371f3dc | 60 | ANTI_SLIP_FUZZY_CONTROL(size_t num_state, size_t num_in, size_t num_out, size_t num_vertex, float samplingTime); |
benson516 | 0:bfcd2371f3dc | 61 | // Assign Parameters |
benson516 | 0:bfcd2371f3dc | 62 | void assign_E_out(float* E_out_in); |
benson516 | 0:bfcd2371f3dc | 63 | // Controller Parameters for each vertex system (k = 1,...,m_vertex) |
benson516 | 0:bfcd2371f3dc | 64 | void assign_ver_K_matrix(float* ver_K_matrix_in); |
benson516 | 0:bfcd2371f3dc | 65 | // |
benson516 | 0:bfcd2371f3dc | 66 | void set_ver_ratio(float ratio_ft_right, float ratio_ft_left); |
benson516 | 0:bfcd2371f3dc | 67 | void fullStateFeedBack_calc(bool enable); |
benson516 | 0:bfcd2371f3dc | 68 | |
benson516 | 0:bfcd2371f3dc | 69 | private: |
benson516 | 0:bfcd2371f3dc | 70 | |
benson516 | 0:bfcd2371f3dc | 71 | vector<float> zeros_n; |
benson516 | 0:bfcd2371f3dc | 72 | vector<float> zeros_p; |
benson516 | 0:bfcd2371f3dc | 73 | vector<float> zeros_q; |
benson516 | 0:bfcd2371f3dc | 74 | vector<float> zeros_nPq; // (n+q) |
benson516 | 0:bfcd2371f3dc | 75 | vector<float> zeros_m_vertex; |
benson516 | 0:bfcd2371f3dc | 76 | // |
benson516 | 0:bfcd2371f3dc | 77 | vector<float> ones_p; |
benson516 | 0:bfcd2371f3dc | 78 | |
benson516 | 0:bfcd2371f3dc | 79 | // Saturation |
benson516 | 0:bfcd2371f3dc | 80 | Saturation SA_r; |
benson516 | 0:bfcd2371f3dc | 81 | Saturation SA_l; |
benson516 | 0:bfcd2371f3dc | 82 | |
benson516 | 0:bfcd2371f3dc | 83 | // Calculate the sys_outputs |
benson516 | 0:bfcd2371f3dc | 84 | void get_sys_outputs(void); // Calculate the sys_outputs from states, by mutiplying E_out |
benson516 | 0:bfcd2371f3dc | 85 | |
benson516 | 0:bfcd2371f3dc | 86 | // Calculate the Integral |
benson516 | 0:bfcd2371f3dc | 87 | void get_integral(bool enable); // Calculate the state_int |
benson516 | 0:bfcd2371f3dc | 88 | |
benson516 | 0:bfcd2371f3dc | 89 | // Concatenate the states and state_int |
benson516 | 0:bfcd2371f3dc | 90 | void get_state_total(void); // Total states, [states; state_int] |
benson516 | 0:bfcd2371f3dc | 91 | |
benson516 | 0:bfcd2371f3dc | 92 | // Utilities |
benson516 | 0:bfcd2371f3dc | 93 | void Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right |
benson516 | 0:bfcd2371f3dc | 94 | vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right |
benson516 | 0:bfcd2371f3dc | 95 | vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b |
benson516 | 0:bfcd2371f3dc | 96 | vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a |
benson516 | 0:bfcd2371f3dc | 97 | // Increment |
benson516 | 0:bfcd2371f3dc | 98 | void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b |
benson516 | 0:bfcd2371f3dc | 99 | |
benson516 | 0:bfcd2371f3dc | 100 | |
benson516 | 0:bfcd2371f3dc | 101 | }; |
benson516 | 0:bfcd2371f3dc | 102 | |
benson516 | 0:bfcd2371f3dc | 103 | #endif |