File content as of revision 1:773d8ae11c1a:

#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

    //
    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


    // 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;

    // 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]



    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 iterateOnce(void);

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 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(void); // 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