Differential drive succeed (Ver. 1.0)
Differential_Drive.cpp
- Committer:
- benson516
- Date:
- 2016-10-26
- Revision:
- 0:644a119c9d8a
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); }