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

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers ANTI_SLIP_FUZZY_CONTROL.h Source File

ANTI_SLIP_FUZZY_CONTROL.h

00001 #ifndef ANTI_SLIP_FUZZY_CONTROL_H
00002 #define ANTI_SLIP_FUZZY_CONTROL_H
00003 //
00004 #include <vector>
00005 #include "FILTER_LIB.h"
00006 
00007 using std::vector;
00008 
00009 //------------------------------------------//
00010 // The template for building a library
00011 //   for control system apllication
00012 //------------------------------------------//
00013 
00014 // The plant is a (p, n, q) system
00015 // Dimensions:
00016 //
00017 // Inputs, u   |   States, x    |   outputs, y
00018 //     p     -->      n        -->      q
00019 //
00020 
00021 class ANTI_SLIP_FUZZY_CONTROL{
00022 public:
00023     // Dimensions
00024     size_t n; // Number of states
00025     size_t p; // Number of inputs of the plant
00026     size_t q; // Number of outputs of the plant, no use in full state feed back case
00027     //
00028     size_t m_vertex; // Number of vertex systems
00029 
00030     float Ts; // Sampling time
00031 
00032     //
00033     bool enable;
00034     bool is_usingFeedForward; // If is_usingFeedForward, Nxd and Nud are used to calculate the x_d and u_d
00035 
00036     // System parameters
00037     vector<vector<float> > E_out; // System output matrix
00038     // Command input matrices
00039     vector<vector<float> > Nxd; // The input matrix for x_d
00040     vector<vector<float> > Nud; // The input matrix for u_d
00041 
00042     // Controller parameters
00043     // Parameters of vertex controller  (k = 1,...,m_vertex)
00044     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
00045 
00046 
00047     // States
00048     // Input signal ---
00049     vector<float> states; // States
00050     vector<float> command; // r, commands
00051     // Output signal ---
00052     vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller
00053 
00054     // Internal states ---
00055     vector<vector<float> > ver_u; // output of the each vertex controller
00056     vector<float> sys_outputs; // The output of the plant, "y", the input of the controller
00057     // Integral state
00058     vector<float> state_int; // x_i
00059     // Total states, [states; state_int]
00060     vector<float> state_total;
00061 
00062     // Equalibrium states for command tracking
00063     vector<float> x_d;
00064     vector<float> u_d;
00065 
00066     //
00067     // The composition ratio of each vertex system
00068     vector<float> ver_ratio; // ver_ratio \in R^m_vertex, its values are in [0, 1]
00069 
00070 
00071 
00072     ANTI_SLIP_FUZZY_CONTROL(size_t num_state, size_t num_in, size_t num_out, size_t num_vertex, float samplingTime);
00073     //
00074     void start();
00075     void pause();
00076     void stop();
00077     void reset();
00078     void reset_integrator(); // Reset the state_int only
00079     //
00080     // Assign Parameters
00081     void assign_E_out(float* E_out_in);
00082     void assign_Nxd(float* Nxd_in);
00083     void assign_Nud(float* Nud_in);
00084     // Controller Parameters for each vertex system (k = 1,...,m_vertex)
00085     void assign_ver_K_matrix(float* ver_K_matrix_in);
00086 
00087     //
00088     void set_ver_ratio(float ratio_ft_right, float ratio_ft_left);
00089     void iterateOnce(void);
00090 
00091 private:
00092 
00093     vector<float> zeros_n;
00094     vector<float> zeros_p;
00095     vector<float> zeros_q;
00096     vector<float> zeros_nPq; // (n+q)
00097     vector<float> zeros_m_vertex;
00098     //
00099     vector<float> ones_p;
00100 
00101     // Saturation
00102     Saturation SA_r;
00103     Saturation SA_l;
00104 
00105     // Calculate the equilibrium states
00106     void get_equilibriumState(void);
00107 
00108     // Calculate the sys_outputs
00109     void get_sys_outputs(void); // Calculate the sys_outputs from states, by mutiplying E_out
00110 
00111     // Calculate the Integral
00112     void get_integral(void); // Calculate the state_int
00113 
00114     // Concatenate the states and state_int
00115     void get_state_total(void); // Total states, [states; state_int]
00116 
00117     // Utilities
00118     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
00119     vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
00120     vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
00121     vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
00122     // Increment
00123     void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b
00124 
00125 
00126 };
00127 
00128 #endif