Diff: ANTI_SLIP_FUZZY_CONTROL.h

Revision:

1:773d8ae11c1a

Parent:

0:bfcd2371f3dc

--- a/ANTI_SLIP_FUZZY_CONTROL.h	Thu Feb 23 11:12:35 2017 +0000
+++ b/ANTI_SLIP_FUZZY_CONTROL.h	Sun Feb 26 05:36:16 2017 +0000
@@ -29,8 +29,16 @@
 
     float Ts; // Sampling time
 
+    //
+    bool enable;
+    bool is_usingFeedForward; // If is_usingFeedForward, Nxd and Nud are used to calculate the x_d and u_d
+
     // System parameters
     vector<vector<float> > E_out; // System output matrix
+    // Command input matrices
+    vector<vector<float> > Nxd; // The input matrix for x_d
+    vector<vector<float> > Nud; // The input matrix for u_d
+
     // 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
@@ -51,6 +59,10 @@
     // Total states, [states; state_int]
     vector<float> state_total;
 
+    // Equalibrium states for command tracking
+    vector<float> x_d;
+    vector<float> u_d;
+
     //
     // The composition ratio of each vertex system
     vector<float> ver_ratio; // ver_ratio \in R^m_vertex, its values are in [0, 1]
@@ -58,13 +70,23 @@
 
 
     ANTI_SLIP_FUZZY_CONTROL(size_t num_state, size_t num_in, size_t num_out, size_t num_vertex, float samplingTime);
+    //
+    void start();
+    void pause();
+    void stop();
+    void reset();
+    void reset_integrator(); // Reset the state_int only
+    //
     // Assign Parameters
     void assign_E_out(float* E_out_in);
+    void assign_Nxd(float* Nxd_in);
+    void assign_Nud(float* Nud_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);
+    void iterateOnce(void);
 
 private:
 
@@ -80,11 +102,14 @@
     Saturation SA_r;
     Saturation SA_l;
 
+    // Calculate the equilibrium states
+    void get_equilibriumState(void);
+
     // 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
+    void get_integral(void); // Calculate the state_int
 
     // Concatenate the states and state_int
     void get_state_total(void); // Total states, [states; state_int]