Differential drive succeed (Ver. 1.0)

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Differential_Drive.cpp Source File

Differential_Drive.cpp

00001 // A motion control for differential drive mobile robot
00002 #include "Differential_Drive.h"
00003 
00004 using std::abs;
00005 using std::vector;
00006 
00007 Diff_Drive::Diff_Drive(float Ts_in, bool Diff_drive_in):
00008             Ts(Ts_in),
00009             Diff_drive(Diff_drive_in),
00010             PID_1(0.0, 0.0, 0.0,Ts_in),
00011             PID_2(0.0, 0.0, 0.0,Ts_in)
00012 {
00013     // Ts = 0.001; // sec., sampling time
00014     
00015     // Ture: differential drive; false: separated control
00016     // Diff_drive = false; 
00017     // 
00018     if (Diff_drive){
00019         // Kp1, Ki1, Kd1, Ka1, Kp2, Ki2, Kd2, Ka2
00020         // float K_1[] = {0.0, 0.0, 0.0, 0.0017, 5.73, 11.46, 0.0, 0.0017};
00021         // float K_1[] = {5.73, 11.46, 0.0, 0.0017, 0.0, 0.0, 0.0, 0.0017};
00022         float K_1[] = {5.73, 11.46, 0.0, 0.0017, 5.73, 11.46, 0.0, 0.0017};
00023         K.assign(K_1,K_1+8);
00024     }else{
00025         // Kp1, Ki1, Kd1, Ka1, Kp2, Ki2, Kd2, Ka2
00026         float K_1[] = {5.73, 11.46, 0.0, 0.0017, 5.73, 11.46, 0.0, 0.0017};
00027         K.assign(K_1,K_1+8);
00028     }
00029     //
00030     
00031     
00032     // Controller parameters
00033     // Control gains
00034     // K.Size = 8
00035     // Kp1, Ki1, Kd1, Ka1, Kp2, Ki2, Kd2, Ka2
00036     // float K_1[] = {5.73, 11.46, 0.0, 0.0017, 5.73, 11.46, 0.0, 0.0017};
00037     // K.assign(K_1,K_1+8);
00038     /*
00039     K.resize(8);
00040     for (size_t i = 0; i < K.size(); ++i){
00041         K[i] = 0.0;
00042     }*/
00043     
00044     // Driver parameters
00045     // Output limit
00046     Vdc = 7.4; // V
00047     voltage_limit_H = Vdc; // +Vdc
00048     voltage_limit_L = -Vdc; // -Vdc
00049     // Input limit
00050     W_max = 4.3633; // 250 deg/s = 4.3633 rad/sec.
00051     W_max_inv = 1.0/W_max;
00052     
00053     // Motor parameters
00054     // Ke = 1.6748; // Speed constant, volt.-sec./rad <- This may be repalced by a nonlinear function f_e(w)
00055     Ke = 1.6;
00056     Kt = Ke; // Torque Constant, Nt./Amp. (Kt almost equals to Ke, Ke >=Kt)
00057     
00058     // Robot parameters
00059     b = 0.1; // m, half of robot's wheel axle
00060     r = 0.045;        // m, wheel radious
00061     //
00062     b_inv = 1.0/b;
00063     r_half = r*0.5;
00064     r_inv = 1.0/r;
00065     r_half_b_inv = r_half*b_inv; // r_half*b_inv
00066     
00067     // States(feedback signals), commands, outputs, 
00068     // States(feedback signals)
00069     V_W.resize(2,0.0);     // Speed-rotaion domain
00070     W1_W2.resize(2,0.0);   // Two-single-wheel domain, W1: right wheel, W2: left wheel
00071     // Commands
00072     Vd_Wd.resize(2, 0.0);     // Input from higher-level commander
00073     W1d_W2d.resize(2, 0.0);
00074     W1dS_W2dS.resize(2, 0.0); // Saturated command for W1 and W2
00075     VdS_WdS.resize(2, 0.0);   // Saturated command for V  and W
00076     // Outputs (voltage command)
00077     UV_UW.resize(2, 0.0);    // Controller output, speed-rotaion domain
00078     U1_U2.resize(2, 0.0);    // Controller output, two-single-wheel domain, U1: right wheel, U2: left wheel
00079     V1_V2.resize(2, 0.0);    // Voltage compensated with back-emf, two-single-wheel domain, V1: right wheel, V2: left wheel
00080     V1S_V2S.resize(2, 0.0);  // Saturated voltage, two-single-wheel domain
00081     
00082     delta_V1_V2.resize(2, 0.0); // The difference between saturated and original voltage command
00083                                // deltaVi = VSi - Vi
00084     delta_VV_VW.resize(2,0.0);  // Speed-rotation domain
00085     
00086                                
00087     // Set PID's parameters                        
00088     /////////////////////
00089     PID_1.Kp = K[0];
00090     PID_1.Ki = K[1];
00091     PID_1.Kd = K[2];
00092     PID_1.Ka = K[3]; // Gain for anti-windup
00093     //
00094     PID_2.Kp = K[4];
00095     PID_2.Ki = K[5];
00096     PID_2.Kd = K[6];
00097     PID_2.Ka = K[7]; // Gain for anti-windup
00098     
00099 }
00100 // T: two-single-wheel domain |-> speed-rotaion domain
00101 //////////////////////
00102 void Diff_Drive::Transform(const vector<float> &V_in, vector<float> &V_out){
00103     V_out[0] = r_half*(V_in[0] + V_in[1]);
00104     V_out[1] = r_half_b_inv*(V_in[0] - V_in[1]);
00105 }
00106 void Diff_Drive::Transform_inv(const vector<float> &V_in, vector<float> &V_out){
00107     V_out[0] = r_inv*(V_in[0] + b*V_in[1]);
00108     V_out[1] = r_inv*(V_in[0] - b*V_in[1]);
00109 }
00110 ////////////////////// end T
00111 
00112 // Main process
00113 ////////////////////
00114 void Diff_Drive::compute(float Vd, float Wd, float W1, float W2){
00115     if (Diff_drive)
00116         compute_DiffDriveMethod(Vd, Wd, W1, W2);
00117     else
00118         compute_SeparatedMethod(Vd, Wd, W1, W2);
00119     
00120 }
00121 void Diff_Drive::compute_SeparatedMethod(float Vd, float Wd, float W1, float W2){
00122 
00123     Vd_Wd[0] = Vd;
00124     Vd_Wd[1] = Wd;
00125 
00126     Transform_inv(Vd_Wd,W1d_W2d);
00127     // Input saturation
00128     // W1dS_W2dS = W1d_W2d; // <- This should be replaced by input saturation
00129     Saturation_input(W1d_W2d, W1dS_W2dS, true);
00130     
00131     // Feedback in
00132     W1_W2[0] = W1;
00133     W1_W2[1] = W2;
00134     
00135     // PI
00136     PID_1.Compute_noWindUP(W1dS_W2dS[0], W1_W2[0]);
00137     PID_2.Compute_noWindUP(W1dS_W2dS[1], W1_W2[1]);
00138     U1_U2[0] = PID_1.output;
00139     U1_U2[1] = PID_2.output;
00140     
00141     // Back emf compensation
00142     V1_V2[0] = U1_U2[0] + func_back_emf(W1_W2[0]);
00143     V1_V2[1] = U1_U2[1] + func_back_emf(W1_W2[1]);
00144     
00145     //Output saturation
00146     Saturation_output(V1_V2, V1S_V2S, delta_V1_V2, true);
00147     
00148     // Anti-windup compensation
00149     PID_1.Anti_windup(delta_V1_V2[0]);
00150     PID_2.Anti_windup(delta_V1_V2[1]);
00151     //
00152     
00153     
00154 }
00155 void Diff_Drive::compute_DiffDriveMethod(float Vd, float Wd, float W1, float W2){
00156     
00157     Vd_Wd[0] = Vd;
00158     Vd_Wd[1] = Wd;
00159 
00160     Transform_inv(Vd_Wd,W1d_W2d);
00161     // Input saturation
00162     // W1dS_W2dS = W1d_W2d; // <- This should be replaced by input saturation
00163     Saturation_input(W1d_W2d, W1dS_W2dS, true);
00164     
00165     // Feedback in
00166     W1_W2[0] = W1;
00167     W1_W2[1] = W2;
00168     
00169     // Speed-rotation domain
00170     /////////////////////////
00171     Transform(W1dS_W2dS, VdS_WdS);
00172     Transform(W1_W2, V_W);
00173     
00174     // PI
00175     PID_1.Compute_noWindUP(VdS_WdS[0], V_W[0]);
00176     PID_2.Compute_noWindUP(VdS_WdS[1], V_W[1]);
00177     UV_UW[0] = PID_1.output;
00178     UV_UW[1] = PID_2.output;
00179     
00180     //
00181     Transform_inv(UV_UW, U1_U2);
00182     ///////////////////////// end Speed-rotation domain
00183     
00184     // Back emf compensation
00185     V1_V2[0] = U1_U2[0] + func_back_emf(W1_W2[0]);
00186     V1_V2[1] = U1_U2[1] + func_back_emf(W1_W2[1]);
00187     
00188     //Output saturation
00189     Saturation_output(V1_V2, V1S_V2S, delta_V1_V2, true);
00190     
00191     
00192     // Speed-rotation domain
00193     ////////////////////////////////
00194     Transform(delta_V1_V2, delta_VV_VW);
00195     
00196     // Anti-windup compensation
00197     PID_1.Anti_windup(delta_VV_VW[0]);
00198     PID_2.Anti_windup(delta_VV_VW[1]);
00199     //
00200     //////////////////////////////// end Speed-rotation domain
00201 }
00202 //////////////////// end Main process
00203 
00204 
00205 // Get results 
00206 float Diff_Drive::get_V1S(){
00207     // Saturated output
00208     return V1S_V2S[0];
00209 }
00210 float Diff_Drive::get_V2S(){
00211     // Saturated output
00212     return V1S_V2S[1];
00213 }
00214 
00215 // Saturation
00216 void Diff_Drive::Saturation_input(const vector<float> &in, vector<float> &out, bool enable){
00217     if (enable){
00218         // out = in;
00219         float alpha_1 = abs(in[0])*W_max_inv;
00220         float alpha_2 = abs(in[1])*W_max_inv;
00221         float alpha_inv = 0.0;
00222         //
00223         if ((alpha_1 > 1) && (alpha_2 > 1)){
00224             if (alpha_1 >= alpha_2){
00225                 alpha_inv = 1/alpha_1;
00226             }else{
00227                 alpha_inv = 1/alpha_2;
00228             }
00229             //
00230             out[0] = alpha_inv*in[0];
00231             out[1] = alpha_inv*in[1];
00232         }else if (alpha_1 > 1){
00233             alpha_inv = 1/alpha_1;
00234             //
00235             out[0] = alpha_inv*in[0];
00236             out[1] = alpha_inv*in[1];
00237         }else if (alpha_2 > 1){
00238             alpha_inv = 1/alpha_2;
00239             //
00240             out[0] = alpha_inv*in[0];
00241             out[1] = alpha_inv*in[1];
00242         }else{
00243             // nothing saturated
00244             out = in;
00245         }
00246     }else{
00247         out = in;
00248     }
00249 }
00250 void Diff_Drive::Saturation_output(const vector<float> &in, vector<float> &out, vector<float> &delta_out, bool enable){
00251     if (enable){
00252         // Output 1
00253         if (in[0] > voltage_limit_H){
00254             delta_out[0] = voltage_limit_H - in[0];
00255             out[0] = voltage_limit_H;
00256         }else if (in[0] <  voltage_limit_L){
00257             delta_out[0] = voltage_limit_L - in[0];
00258             out[0] = voltage_limit_L;
00259         }else{
00260             out[0] = in[0];
00261             //
00262             delta_out[0] = 0.0;
00263         }
00264         // Output 2
00265         if (in[1] > voltage_limit_H){
00266             delta_out[1] = voltage_limit_H - in[1];
00267             out[1] = voltage_limit_H;
00268         }else if (in[1] <  voltage_limit_L){
00269             delta_out[1] = voltage_limit_L - in[1];
00270             out[1] = voltage_limit_L;
00271         }else{
00272             out[1] = in[1];
00273             //
00274             delta_out[1] = 0.0;
00275         }
00276     }else{
00277         out = in;
00278         //
00279         delta_out[0] = 0.0;
00280         delta_out[1] = 0.0;
00281     }
00282 }
00283 
00284 // Back emf as the function of rotational speed
00285 float Diff_Drive::func_back_emf(float W_in){
00286     return (Ke*W_in);
00287 }