Diff: ANTI_SLIP_FUZZY_CONTROL.cpp

Revision:

1:773d8ae11c1a

Parent:

0:bfcd2371f3dc

--- a/ANTI_SLIP_FUZZY_CONTROL.cpp	Thu Feb 23 11:12:35 2017 +0000
+++ b/ANTI_SLIP_FUZZY_CONTROL.cpp	Sun Feb 26 05:36:16 2017 +0000
@@ -19,10 +19,19 @@
 ANTI_SLIP_FUZZY_CONTROL::ANTI_SLIP_FUZZY_CONTROL(size_t num_state, size_t num_in, size_t num_out, size_t num_vertex, float samplingTime):
         n(num_state), p(num_in), q(num_out), m_vertex(num_vertex), Ts(samplingTime),
         E_out(num_out, vector<float>(num_state,0.0)),
+        Nxd(num_state, vector<float>(num_in,0.0)),
+        Nud(num_in, vector<float>(num_in,0.0)),
         ver_K_matrix(num_vertex ,vector<vector<float> >(num_in, vector<float>( (num_state + num_out), 0.0)) ),
         SA_r(1.0, 0.0),
         SA_l(1.0, 0.0)
 {
+
+    // To enble, run *.start() function
+    enable = false;
+    // If is_usingFeedForward, Nxd and Nud are used to calculate the x_d and u_d
+    is_usingFeedForward = false;
+
+
     // Normally, q = p
     if (q > p)
         q = p;
@@ -50,12 +59,59 @@
     // Total states, [states; state_int]
     state_total = zeros_nPq;
 
+    // Equalibrium states for command tracking
+    x_d = zeros_n;
+    u_d = zeros_p;
 
     // The composition ratio of each vertex system
     ver_ratio = zeros_m_vertex; // ver_ratio \in R^m_vertex, its values are in [0, 1]
-    ver_ratio[3] = 1.0; // Vertex of no-slip system
+    ver_ratio[3] = 1.0; // Ratio for no-slip system
 
 }
+void ANTI_SLIP_FUZZY_CONTROL::start(){
+    enable = true;
+}
+void ANTI_SLIP_FUZZY_CONTROL::pause(){
+    enable = false;
+}
+void ANTI_SLIP_FUZZY_CONTROL::stop(){
+    if (!enable){
+        return;
+    }
+    enable = false;
+    // Reset
+    reset();
+}
+void ANTI_SLIP_FUZZY_CONTROL::reset(){
+    //
+    // States
+    // Input signal ---
+    states = zeros_n;
+    command = zeros_q; // r, commands, q = p
+    // Output signal ---
+    sys_inputs = zeros_p;
+
+    // Internal states ---
+    // ver_u.assign(m_vertex, zeros_p);
+    sys_outputs = zeros_q;
+    // Integral state
+    state_int = zeros_q;
+    // Total states, [states; state_int]
+    state_total = zeros_nPq;
+
+    // Equalibrium states for command tracking
+    x_d = zeros_n;
+    u_d = zeros_p;
+
+    // The composition ratio of each vertex system
+    ver_ratio = zeros_m_vertex; // ver_ratio \in R^m_vertex, its values are in [0, 1]
+    ver_ratio[3] = 1.0; // Ratio for no-slip system
+
+}
+void ANTI_SLIP_FUZZY_CONTROL::reset_integrator(){ // Reset the state_int only
+    // Integral state
+    state_int = zeros_q;
+}
 // Assign Parameters
 void ANTI_SLIP_FUZZY_CONTROL::assign_E_out(float* E_out_in){
     // E_out_in is the pointer of a mutidimentional array with size q by n
@@ -69,6 +125,34 @@
         }
     }
 }
+void ANTI_SLIP_FUZZY_CONTROL::assign_Nxd(float* Nxd_in){
+    // Nxd_in is the pointer of a mutidimentional array with size n by p
+    Nxd.assign(n, zeros_p);
+    //
+    for (size_t i = 0; i < n; ++i){
+        for (size_t j = 0; j < p; ++j){
+            // Nxd[i][j] = Nxd_in[i][j];
+            Nxd[i][j] = *Nxd_in;
+            Nxd_in++;
+        }
+    }
+    //
+    is_usingFeedForward = true;
+}
+void ANTI_SLIP_FUZZY_CONTROL::assign_Nud(float* Nud_in){
+    // Nxd_in is the pointer of a mutidimentional array with size p by p
+    Nud.assign(p, zeros_p);
+    //
+    for (size_t i = 0; i < p; ++i){
+        for (size_t j = 0; j < p; ++j){
+            // Nud[i][j] = Nud_in[i][j];
+            Nud[i][j] = *Nud_in;
+            Nud_in++;
+        }
+    }
+    //
+    is_usingFeedForward = true;
+}
 // 1st controller Parameters (no-slip plant, sys_dVs)
 void ANTI_SLIP_FUZZY_CONTROL::assign_ver_K_matrix(float* ver_K_matrix_in){
     // ver_K_matrix_in is the pointer of a list of mutidimentional array with size p by (n+q)
@@ -106,86 +190,99 @@
     ver_ratio[3] = ratio_ft_right       * ratio_ft_left;
 }
 //
-void ANTI_SLIP_FUZZY_CONTROL::fullStateFeedBack_calc(bool enable){
+void ANTI_SLIP_FUZZY_CONTROL::iterateOnce(void){
 
-    // Control law
-    if (enable){
+    if(!enable){
+        return;
+    }
+
+    //
+    if (is_usingFeedForward){
+        get_equilibriumState();
         // Get the state_total
         get_state_total();
-
-
-        /*
-        // Slower solution, record the outout of each vertex controller
-        sys_inputs = zeros_p;
-        //
-        for (size_t k = 0; k < m_vertex; ++k){
-            ver_u[k] = Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), -1.0 );
-            Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( ver_u[k], ver_ratio[k]), false); // +=
-        }
-        */
-
-
-
-        // Faster solution, no recording the output of each vertex controller
-        sys_inputs = zeros_p;
-        //
-
-        for (size_t k = 0; k < m_vertex; ++k){
-            // sys_inputs -= ver_ratio[k]*(ver_K_matrix[k]*state_total);
-            Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), ver_ratio[k] ), true); // -=
-        }
-
-
-
-        /*
-        for (size_t k = 3; k < 4; ++k){
-            // sys_inputs -= ver_ratio[k]*(ver_K_matrix[k]*state_total);
-            Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), ver_ratio[k] ), true); // -=
-        }
-        */
-
-
-
-        //
+        // u = u_d + yc;
+        sys_inputs = u_d;
     }else{
-        state_total = zeros_nPq;
+        // Get the state_total
+        get_state_total();
+        // u = yc;
         sys_inputs = zeros_p;
     }
 
+
+    /*
+    // Slower solution, record the outout of each vertex controller
+    sys_inputs = zeros_p;
+    //
+    for (size_t k = 0; k < m_vertex; ++k){
+        ver_u[k] = Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), -1.0 );
+        Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( ver_u[k], ver_ratio[k]), false); // +=
+    }
+    */
+
+    // Faster solution, no recording the output of each vertex controller
+    // sys_inputs = zeros_p;
+    //
+    for (size_t k = 0; k < m_vertex; ++k){
+        // sys_inputs -= ver_ratio[k]*(ver_K_matrix[k]*state_total);
+        Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), ver_ratio[k] ), true); // -=
+    }
+
+    /*
+    // Add the u_d
+    if (is_usingFeedForward){
+        Get_VectorIncrement(sys_inputs, u_d, false); // +=
+    }
+    */
+
     // Integral action
-    get_integral(enable);
+    get_integral();
 }
 
 // Private functions
+// Calculate the equilibrium states
+void ANTI_SLIP_FUZZY_CONTROL::get_equilibriumState(void){
+    Mat_multiply_Vec(x_d, Nxd, command);
+    Mat_multiply_Vec(u_d, Nud, command);
+}
 // Calculate the sys_outputs
 void ANTI_SLIP_FUZZY_CONTROL::get_sys_outputs(void){ // Calculate the sys_outputs from states, by mutiplying E_out
     // sys_outputs = E_out*states
-    // Mat_multiply_Vec(sys_outputs, E_out, states);
-    sys_outputs = Mat_multiply_Vec(E_out, states);
+    Mat_multiply_Vec(sys_outputs, E_out, states);
+    // sys_outputs = Mat_multiply_Vec(E_out, states);
 }
 // Calculate the Integral
-void ANTI_SLIP_FUZZY_CONTROL::get_integral(bool enable){ // Calculate the state_int
+void ANTI_SLIP_FUZZY_CONTROL::get_integral(){ // Calculate the state_int
+    if(!enable){
+        return;
+    }
     //
     // Calculate the sys_outputs
     get_sys_outputs();
-
+    //
     // Integral action
     // state_int += sys_outputs - command
-    if (enable){
-        Get_VectorIncrement(state_int, Get_VectorScalarMultiply(Get_VectorPlus(sys_outputs,command,true),Ts) ,false); // +=
-    }else{
-        state_int = zeros_q;
-    }
+    Get_VectorIncrement(state_int, Get_VectorScalarMultiply(Get_VectorPlus(sys_outputs,command,true),Ts) ,false); // +=
 }
 // Concatenate the state and state_int
 void ANTI_SLIP_FUZZY_CONTROL::get_state_total(void){ // Total states, [states; state_int]
     //
     size_t idx = 0;
+
     // states
-    for (size_t i = 0; i < n; ++i){
-        state_total[idx] = states[i];
-        idx++;
+    if (is_usingFeedForward){
+        for (size_t i = 0; i < n; ++i){
+            state_total[idx] = states[i] - x_d[i];
+            idx++;
+        }
+    }else{
+        for (size_t i = 0; i < n; ++i){
+            state_total[idx] = states[i];
+            idx++;
+        }
     }
+
     // state_int
     for (size_t i = 0; i < q; ++i){
         state_total[idx] = state_int[i];