File content as of revision 0:644a119c9d8a:

// A motion control for differential drive mobile robot
#include "Differential_Drive.h"

using std::abs;
using std::vector;

Diff_Drive::Diff_Drive(float Ts_in, bool Diff_drive_in):
            Ts(Ts_in),
            Diff_drive(Diff_drive_in),
            PID_1(0.0, 0.0, 0.0,Ts_in),
            PID_2(0.0, 0.0, 0.0,Ts_in)
{
    // Ts = 0.001; // sec., sampling time
    
    // Ture: differential drive; false: separated control
    // Diff_drive = false; 
    // 
    if (Diff_drive){
        // Kp1, Ki1, Kd1, Ka1, Kp2, Ki2, Kd2, Ka2
        // float K_1[] = {0.0, 0.0, 0.0, 0.0017, 5.73, 11.46, 0.0, 0.0017};
        // float K_1[] = {5.73, 11.46, 0.0, 0.0017, 0.0, 0.0, 0.0, 0.0017};
        float K_1[] = {5.73, 11.46, 0.0, 0.0017, 5.73, 11.46, 0.0, 0.0017};
        K.assign(K_1,K_1+8);
    }else{
        // Kp1, Ki1, Kd1, Ka1, Kp2, Ki2, Kd2, Ka2
        float K_1[] = {5.73, 11.46, 0.0, 0.0017, 5.73, 11.46, 0.0, 0.0017};
        K.assign(K_1,K_1+8);
    }
    //
    
    
    // Controller parameters
    // Control gains
    // K.Size = 8
    // Kp1, Ki1, Kd1, Ka1, Kp2, Ki2, Kd2, Ka2
    // float K_1[] = {5.73, 11.46, 0.0, 0.0017, 5.73, 11.46, 0.0, 0.0017};
    // K.assign(K_1,K_1+8);
    /*
    K.resize(8);
    for (size_t i = 0; i < K.size(); ++i){
        K[i] = 0.0;
    }*/
    
    // Driver parameters
    // Output limit
    Vdc = 7.4; // V
    voltage_limit_H = Vdc; // +Vdc
    voltage_limit_L = -Vdc; // -Vdc
    // Input limit
    W_max = 4.3633; // 250 deg/s = 4.3633 rad/sec.
    W_max_inv = 1.0/W_max;
    
    // Motor parameters
    // Ke = 1.6748; // Speed constant, volt.-sec./rad <- This may be repalced by a nonlinear function f_e(w)
    Ke = 1.6;
    Kt = Ke; // Torque Constant, Nt./Amp. (Kt almost equals to Ke, Ke >=Kt)
    
    // Robot parameters
    b = 0.1; // m, half of robot's wheel axle
    r = 0.045;        // m, wheel radious
    //
    b_inv = 1.0/b;
    r_half = r*0.5;
    r_inv = 1.0/r;
    r_half_b_inv = r_half*b_inv; // r_half*b_inv
    
    // States(feedback signals), commands, outputs, 
    // States(feedback signals)
    V_W.resize(2,0.0);     // Speed-rotaion domain
    W1_W2.resize(2,0.0);   // Two-single-wheel domain, W1: right wheel, W2: left wheel
    // Commands
    Vd_Wd.resize(2, 0.0);     // Input from higher-level commander
    W1d_W2d.resize(2, 0.0);
    W1dS_W2dS.resize(2, 0.0); // Saturated command for W1 and W2
    VdS_WdS.resize(2, 0.0);   // Saturated command for V  and W
    // Outputs (voltage command)
    UV_UW.resize(2, 0.0);    // Controller output, speed-rotaion domain
    U1_U2.resize(2, 0.0);    // Controller output, two-single-wheel domain, U1: right wheel, U2: left wheel
    V1_V2.resize(2, 0.0);    // Voltage compensated with back-emf, two-single-wheel domain, V1: right wheel, V2: left wheel
    V1S_V2S.resize(2, 0.0);  // Saturated voltage, two-single-wheel domain
    
    delta_V1_V2.resize(2, 0.0); // The difference between saturated and original voltage command
                               // deltaVi = VSi - Vi
    delta_VV_VW.resize(2,0.0);  // Speed-rotation domain
    
                               
    // Set PID's parameters                        
    /////////////////////
    PID_1.Kp = K[0];
    PID_1.Ki = K[1];
    PID_1.Kd = K[2];
    PID_1.Ka = K[3]; // Gain for anti-windup
    //
    PID_2.Kp = K[4];
    PID_2.Ki = K[5];
    PID_2.Kd = K[6];
    PID_2.Ka = K[7]; // Gain for anti-windup
    
}
// T: two-single-wheel domain |-> speed-rotaion domain
//////////////////////
void Diff_Drive::Transform(const vector<float> &V_in, vector<float> &V_out){
    V_out[0] = r_half*(V_in[0] + V_in[1]);
    V_out[1] = r_half_b_inv*(V_in[0] - V_in[1]);
}
void Diff_Drive::Transform_inv(const vector<float> &V_in, vector<float> &V_out){
    V_out[0] = r_inv*(V_in[0] + b*V_in[1]);
    V_out[1] = r_inv*(V_in[0] - b*V_in[1]);
}
////////////////////// end T

// Main process
////////////////////
void Diff_Drive::compute(float Vd, float Wd, float W1, float W2){
    if (Diff_drive)
        compute_DiffDriveMethod(Vd, Wd, W1, W2);
    else
        compute_SeparatedMethod(Vd, Wd, W1, W2);
    
}
void Diff_Drive::compute_SeparatedMethod(float Vd, float Wd, float W1, float W2){

    Vd_Wd[0] = Vd;
    Vd_Wd[1] = Wd;

    Transform_inv(Vd_Wd,W1d_W2d);
    // Input saturation
    // W1dS_W2dS = W1d_W2d; // <- This should be replaced by input saturation
    Saturation_input(W1d_W2d, W1dS_W2dS, true);
    
    // Feedback in
    W1_W2[0] = W1;
    W1_W2[1] = W2;
    
    // PI
    PID_1.Compute_noWindUP(W1dS_W2dS[0], W1_W2[0]);
    PID_2.Compute_noWindUP(W1dS_W2dS[1], W1_W2[1]);
    U1_U2[0] = PID_1.output;
    U1_U2[1] = PID_2.output;
    
    // Back emf compensation
    V1_V2[0] = U1_U2[0] + func_back_emf(W1_W2[0]);
    V1_V2[1] = U1_U2[1] + func_back_emf(W1_W2[1]);
    
    //Output saturation
    Saturation_output(V1_V2, V1S_V2S, delta_V1_V2, true);
    
    // Anti-windup compensation
    PID_1.Anti_windup(delta_V1_V2[0]);
    PID_2.Anti_windup(delta_V1_V2[1]);
    //
    
    
}
void Diff_Drive::compute_DiffDriveMethod(float Vd, float Wd, float W1, float W2){
    
    Vd_Wd[0] = Vd;
    Vd_Wd[1] = Wd;

    Transform_inv(Vd_Wd,W1d_W2d);
    // Input saturation
    // W1dS_W2dS = W1d_W2d; // <- This should be replaced by input saturation
    Saturation_input(W1d_W2d, W1dS_W2dS, true);
    
    // Feedback in
    W1_W2[0] = W1;
    W1_W2[1] = W2;
    
    // Speed-rotation domain
    /////////////////////////
    Transform(W1dS_W2dS, VdS_WdS);
    Transform(W1_W2, V_W);
    
    // PI
    PID_1.Compute_noWindUP(VdS_WdS[0], V_W[0]);
    PID_2.Compute_noWindUP(VdS_WdS[1], V_W[1]);
    UV_UW[0] = PID_1.output;
    UV_UW[1] = PID_2.output;
    
    //
    Transform_inv(UV_UW, U1_U2);
    ///////////////////////// end Speed-rotation domain
    
    // Back emf compensation
    V1_V2[0] = U1_U2[0] + func_back_emf(W1_W2[0]);
    V1_V2[1] = U1_U2[1] + func_back_emf(W1_W2[1]);
    
    //Output saturation
    Saturation_output(V1_V2, V1S_V2S, delta_V1_V2, true);
    
    
    // Speed-rotation domain
    ////////////////////////////////
    Transform(delta_V1_V2, delta_VV_VW);
    
    // Anti-windup compensation
    PID_1.Anti_windup(delta_VV_VW[0]);
    PID_2.Anti_windup(delta_VV_VW[1]);
    //
    //////////////////////////////// end Speed-rotation domain
}
//////////////////// end Main process


// Get results 
float Diff_Drive::get_V1S(){
    // Saturated output
    return V1S_V2S[0];
}
float Diff_Drive::get_V2S(){
    // Saturated output
    return V1S_V2S[1];
}

// Saturation
void Diff_Drive::Saturation_input(const vector<float> &in, vector<float> &out, bool enable){
    if (enable){
        // out = in;
        float alpha_1 = abs(in[0])*W_max_inv;
        float alpha_2 = abs(in[1])*W_max_inv;
        float alpha_inv = 0.0;
        //
        if ((alpha_1 > 1) && (alpha_2 > 1)){
            if (alpha_1 >= alpha_2){
                alpha_inv = 1/alpha_1;
            }else{
                alpha_inv = 1/alpha_2;
            }
            //
            out[0] = alpha_inv*in[0];
            out[1] = alpha_inv*in[1];
        }else if (alpha_1 > 1){
            alpha_inv = 1/alpha_1;
            //
            out[0] = alpha_inv*in[0];
            out[1] = alpha_inv*in[1];
        }else if (alpha_2 > 1){
            alpha_inv = 1/alpha_2;
            //
            out[0] = alpha_inv*in[0];
            out[1] = alpha_inv*in[1];
        }else{
            // nothing saturated
            out = in;
        }
    }else{
        out = in;
    }
}
void Diff_Drive::Saturation_output(const vector<float> &in, vector<float> &out, vector<float> &delta_out, bool enable){
    if (enable){
        // Output 1
        if (in[0] > voltage_limit_H){
            delta_out[0] = voltage_limit_H - in[0];
            out[0] = voltage_limit_H;
        }else if (in[0] <  voltage_limit_L){
            delta_out[0] = voltage_limit_L - in[0];
            out[0] = voltage_limit_L;
        }else{
            out[0] = in[0];
            //
            delta_out[0] = 0.0;
        }
        // Output 2
        if (in[1] > voltage_limit_H){
            delta_out[1] = voltage_limit_H - in[1];
            out[1] = voltage_limit_H;
        }else if (in[1] <  voltage_limit_L){
            delta_out[1] = voltage_limit_L - in[1];
            out[1] = voltage_limit_L;
        }else{
            out[1] = in[1];
            //
            delta_out[1] = 0.0;
        }
    }else{
        out = in;
        //
        delta_out[0] = 0.0;
        delta_out[1] = 0.0;
    }
}

// Back emf as the function of rotational speed
float Diff_Drive::func_back_emf(float W_in){
    return (Ke*W_in);
}