Chun Feng Huang
Differential drive succeed (Ver. 1.0)
Diff: Differential_Drive.cpp
- Revision:
- 0:644a119c9d8a
--- /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