Diff: ANTI_SLIP_FUZZY_CONTROL.h

Revision:

0:bfcd2371f3dc

Child:

1:773d8ae11c1a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ANTI_SLIP_FUZZY_CONTROL.h	Thu Feb 23 11:12:35 2017 +0000
@@ -0,0 +1,103 @@
+#ifndef ANTI_SLIP_FUZZY_CONTROL_H
+#define ANTI_SLIP_FUZZY_CONTROL_H
+//
+#include <vector>
+#include "FILTER_LIB.h"
+
+using std::vector;
+
+//------------------------------------------//
+// The template for building a library
+//   for control system apllication
+//------------------------------------------//
+
+// The plant is a (p, n, q) system
+// Dimensions:
+//
+// Inputs, u   |   States, x    |   outputs, y
+//     p     -->      n        -->      q
+//
+
+class ANTI_SLIP_FUZZY_CONTROL{
+public:
+    // Dimensions
+    size_t n; // Number of states
+    size_t p; // Number of inputs of the plant
+    size_t q; // Number of outputs of the plant, no use in full state feed back case
+    //
+    size_t m_vertex; // Number of vertex systems
+
+    float Ts; // Sampling time
+
+    // System parameters
+    vector<vector<float> > E_out; // System output matrix
+    // Controller parameters
+    // Parameters of vertex controller  (k = 1,...,m_vertex)
+    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
+
+
+    // States
+    // Input signal ---
+    vector<float> states; // States
+    vector<float> command; // r, commands
+    // Output signal ---
+    vector<float> sys_inputs; // The inputs of the plant, "u", the "output" of the controller
+
+    // Internal states ---
+    vector<vector<float> > ver_u; // output of the each vertex controller
+    vector<float> sys_outputs; // The output of the plant, "y", the input of the controller
+    // Integral state
+    vector<float> state_int; // x_i
+    // Total states, [states; state_int]
+    vector<float> state_total;
+
+    //
+    // The composition ratio of each vertex system
+    vector<float> ver_ratio; // ver_ratio \in R^m_vertex, its values are in [0, 1]
+
+
+
+    ANTI_SLIP_FUZZY_CONTROL(size_t num_state, size_t num_in, size_t num_out, size_t num_vertex, float samplingTime);
+    // Assign Parameters
+    void assign_E_out(float* E_out_in);
+    // Controller Parameters for each vertex system (k = 1,...,m_vertex)
+    void assign_ver_K_matrix(float* ver_K_matrix_in);
+    //
+    void set_ver_ratio(float ratio_ft_right, float ratio_ft_left);
+    void fullStateFeedBack_calc(bool enable);
+
+private:
+
+    vector<float> zeros_n;
+    vector<float> zeros_p;
+    vector<float> zeros_q;
+    vector<float> zeros_nPq; // (n+q)
+    vector<float> zeros_m_vertex;
+    //
+    vector<float> ones_p;
+
+    // Saturation
+    Saturation SA_r;
+    Saturation SA_l;
+
+    // Calculate the sys_outputs
+    void get_sys_outputs(void); // Calculate the sys_outputs from states, by mutiplying E_out
+
+    // Calculate the Integral
+    void get_integral(bool enable); // Calculate the state_int
+
+    // Concatenate the states and state_int
+    void get_state_total(void); // Total states, [states; state_int]
+
+    // Utilities
+    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
+    vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
+    vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
+    vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
+    // Increment
+    void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b
+
+
+};
+
+#endif