Chun Feng Huang / ANTI_SLIP_FUZZY_CONTROL

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?

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