Chun Feng Huang / ANTI_SLIP_FUZZY_CONTROL

A new type of anti-slip controller based on TS fuzzy models

ANTI_SLIP_FUZZY_CONTROL.h@1:773d8ae11c1a, 2017-02-26 (annotated)

Committer:
benson516
Date:
Sun Feb 26 05:36:16 2017 +0000
Revision:
1:773d8ae11c1a
Parent:
0:bfcd2371f3dc 
Ver. 1.00

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 1:773d8ae11c1a 32 //
benson516 1:773d8ae11c1a 33 bool enable;
benson516 1:773d8ae11c1a 34 bool is_usingFeedForward; // If is_usingFeedForward, Nxd and Nud are used to calculate the x_d and u_d
benson516 1:773d8ae11c1a 35
benson516 0:bfcd2371f3dc 36 // System parameters
benson516 0:bfcd2371f3dc 37 vector<vector<float> > E_out; // System output matrix
benson516 1:773d8ae11c1a 38 // Command input matrices
benson516 1:773d8ae11c1a 39 vector<vector<float> > Nxd; // The input matrix for x_d
benson516 1:773d8ae11c1a 40 vector<vector<float> > Nud; // The input matrix for u_d
benson516 1:773d8ae11c1a 41
benson516 0:bfcd2371f3dc 42 // Controller parameters
benson516 0:bfcd2371f3dc 43 // Parameters of vertex controller (k = 1,...,m_vertex)
benson516 0:bfcd2371f3dc 44 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 45
benson516 0:bfcd2371f3dc 46
benson516 0:bfcd2371f3dc 47 // States
benson516 0:bfcd2371f3dc 48 // Input signal ---
benson516 0:bfcd2371f3dc 49 vector<float> states; // States
benson516 0:bfcd2371f3dc 50 vector<float> command; // r, commands
benson516 0:bfcd2371f3dc 51 // Output signal ---
benson516 0:bfcd2371f3dc 52 vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller
benson516 0:bfcd2371f3dc 53
benson516 0:bfcd2371f3dc 54 // Internal states ---
benson516 0:bfcd2371f3dc 55 vector<vector<float> > ver_u; // output of the each vertex controller
benson516 0:bfcd2371f3dc 56 vector<float> sys_outputs; // The output of the plant, "y", the input of the controller
benson516 0:bfcd2371f3dc 57 // Integral state
benson516 0:bfcd2371f3dc 58 vector<float> state_int; // x_i
benson516 0:bfcd2371f3dc 59 // Total states, [states; state_int]
benson516 0:bfcd2371f3dc 60 vector<float> state_total;
benson516 0:bfcd2371f3dc 61
benson516 1:773d8ae11c1a 62 // Equalibrium states for command tracking
benson516 1:773d8ae11c1a 63 vector<float> x_d;
benson516 1:773d8ae11c1a 64 vector<float> u_d;
benson516 1:773d8ae11c1a 65
benson516 0:bfcd2371f3dc 66 //
benson516 0:bfcd2371f3dc 67 // The composition ratio of each vertex system
benson516 0:bfcd2371f3dc 68 vector<float> ver_ratio; // ver_ratio \in R^m_vertex, its values are in [0, 1]
benson516 0:bfcd2371f3dc 69
benson516 0:bfcd2371f3dc 70
benson516 0:bfcd2371f3dc 71
benson516 0:bfcd2371f3dc 72 ANTI_SLIP_FUZZY_CONTROL(size_t num_state, size_t num_in, size_t num_out, size_t num_vertex, float samplingTime);
benson516 1:773d8ae11c1a 73 //
benson516 1:773d8ae11c1a 74 void start();
benson516 1:773d8ae11c1a 75 void pause();
benson516 1:773d8ae11c1a 76 void stop();
benson516 1:773d8ae11c1a 77 void reset();
benson516 1:773d8ae11c1a 78 void reset_integrator(); // Reset the state_int only
benson516 1:773d8ae11c1a 79 //
benson516 0:bfcd2371f3dc 80 // Assign Parameters
benson516 0:bfcd2371f3dc 81 void assign_E_out(float* E_out_in);
benson516 1:773d8ae11c1a 82 void assign_Nxd(float* Nxd_in);
benson516 1:773d8ae11c1a 83 void assign_Nud(float* Nud_in);
benson516 0:bfcd2371f3dc 84 // Controller Parameters for each vertex system (k = 1,...,m_vertex)
benson516 0:bfcd2371f3dc 85 void assign_ver_K_matrix(float* ver_K_matrix_in);
benson516 1:773d8ae11c1a 86
benson516 0:bfcd2371f3dc 87 //
benson516 0:bfcd2371f3dc 88 void set_ver_ratio(float ratio_ft_right, float ratio_ft_left);
benson516 1:773d8ae11c1a 89 void iterateOnce(void);
benson516 0:bfcd2371f3dc 90
benson516 0:bfcd2371f3dc 91 private:
benson516 0:bfcd2371f3dc 92
benson516 0:bfcd2371f3dc 93 vector<float> zeros_n;
benson516 0:bfcd2371f3dc 94 vector<float> zeros_p;
benson516 0:bfcd2371f3dc 95 vector<float> zeros_q;
benson516 0:bfcd2371f3dc 96 vector<float> zeros_nPq; // (n+q)
benson516 0:bfcd2371f3dc 97 vector<float> zeros_m_vertex;
benson516 0:bfcd2371f3dc 98 //
benson516 0:bfcd2371f3dc 99 vector<float> ones_p;
benson516 0:bfcd2371f3dc 100
benson516 0:bfcd2371f3dc 101 // Saturation
benson516 0:bfcd2371f3dc 102 Saturation SA_r;
benson516 0:bfcd2371f3dc 103 Saturation SA_l;
benson516 0:bfcd2371f3dc 104
benson516 1:773d8ae11c1a 105 // Calculate the equilibrium states
benson516 1:773d8ae11c1a 106 void get_equilibriumState(void);
benson516 1:773d8ae11c1a 107
benson516 0:bfcd2371f3dc 108 // Calculate the sys_outputs
benson516 0:bfcd2371f3dc 109 void get_sys_outputs(void); // Calculate the sys_outputs from states, by mutiplying E_out
benson516 0:bfcd2371f3dc 110
benson516 0:bfcd2371f3dc 111 // Calculate the Integral
benson516 1:773d8ae11c1a 112 void get_integral(void); // Calculate the state_int
benson516 0:bfcd2371f3dc 113
benson516 0:bfcd2371f3dc 114 // Concatenate the states and state_int
benson516 0:bfcd2371f3dc 115 void get_state_total(void); // Total states, [states; state_int]
benson516 0:bfcd2371f3dc 116
benson516 0:bfcd2371f3dc 117 // Utilities
benson516 0:bfcd2371f3dc 118 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 119 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 120 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 121 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
benson516 0:bfcd2371f3dc 122 // Increment
benson516 0:bfcd2371f3dc 123 void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b
benson516 0:bfcd2371f3dc 124
benson516 0:bfcd2371f3dc 125
benson516 0:bfcd2371f3dc 126 };
benson516 0:bfcd2371f3dc 127
benson516 0:bfcd2371f3dc 128 #endif