Differential drive succeed (Ver. 1.0)
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sat Jul 23 2022 02:02:34 by
1.7.2