Differential drive succeed (Ver. 1.0)
Revision 0:644a119c9d8a, committed 2016-10-26
- Comitter:
- benson516
- Date:
- Wed Oct 26 17:56:15 2016 +0000
- Commit message:
- Differential drive succeed (Ver. 1.0)
Changed in this revision
Differential_Drive.cpp | Show annotated file Show diff for this revision Revisions of this file |
Differential_Drive.h | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Differential_Drive.cpp Wed Oct 26 17:56:15 2016 +0000 @@ -0,0 +1,287 @@ +// 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); +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Differential_Drive.h Wed Oct 26 17:56:15 2016 +0000 @@ -0,0 +1,96 @@ +// A motion control for differential drive mobile robot +// Designed by Chun-Feng Huang (Benson516) + +#ifndef DIFFERENTIL_DRIVE_H +#define DIFFERENTIL_DRIVE_H +// Begine the library +//////////////////////// + +// #include "mbed.h" +#include <vector> +#include <cmath> // for abs() +#include "PID.h" + +using std::abs; +using std::vector; + +class Diff_Drive{ + float Ts; // sec., sampling time + + ///////////////////////// + + // Controller parameters + // Control gains + // K.Size = 8 + // Kp1, Ki1, Kd1, Ka1, Kp2, Ki2, Kd2, Ka2 + vector<float> K; + + // Driver parameters + // Output limit + float Vdc; // Volt. + float voltage_limit_H; // +Vdc + float voltage_limit_L; // -Vdc + // Input limit + float W_max,W_max_inv; // rad/s + + // Motor parameters + float Ke; // Speed constant, volt.-sec./rad <- This may be repalced by a nonlinear function f_e(w) + float Kt; // Torque Constant, Nt./Amp. (Ke >=Kt) + + // Robot parameters + float b, b_inv; // m, half of robot's wheel axle + float r, r_half, r_inv; // m, wheel radious + float r_half_b_inv; // r_half*b_inv + + // States(feedback signals), commands, outputs, + // States(feedback signals) + vector<float> V_W; // Speed-rotaion domain + vector<float> W1_W2; // Two-single-wheel domain, W1: right wheel, W2: left wheel + // Commands + vector<float> Vd_Wd; // Input from higher-level commander + vector<float> W1d_W2d; + vector<float> W1dS_W2dS; // Saturated command for W1 and W2 + vector<float> VdS_WdS; // Saturated command for V and W + // Outputs (voltage command) + vector<float> UV_UW; // Controller output, speed-rotaion domain + vector<float> U1_U2; // Controller output, two-single-wheel domain, U1: right wheel, U2: left wheel + vector<float> V1_V2; // Voltage compensated with back-emf, two-single-wheel domain, V1: right wheel, V2: left wheel + vector<float> V1S_V2S; // Saturated voltage, two-single-wheel domain + + vector<float> delta_V1_V2, delta_VV_VW; // The difference between saturated and original voltage command + // deltaVi = VSi - Vi + + // PID controller + PID PID_1,PID_2; + +public: + Diff_Drive(float Ts_in, bool Diff_drive_in); + + // + bool Diff_drive; // Ture: differential drive; false: separated control + // + + // T: two-single-wheel domain |-> speed-rotaion domain + void Transform(const vector<float> &V_in, vector<float> &V_out); + void Transform_inv(const vector<float> &V_in, vector<float> &V_out); // Inverse transformation + + // Main process + void compute(float Vd, float Wd, float W1, float W2); + void compute_SeparatedMethod(float Vd, float Wd, float W1, float W2); + void compute_DiffDriveMethod(float Vd, float Wd, float W1, float W2); + // Get results + float get_V1S(); + float get_V2S(); + + // Saturation + void Saturation_input(const vector<float> &in, vector<float> &out, bool enable); + void Saturation_output(const vector<float> &in, vector<float> &out,vector<float> &delta_out, bool enable); + + // Back emf as the function of rotational speed + float func_back_emf(float W_in); + +}; + + +//////////////////////// end Begine the library +#endif