Chun Feng Huang / Differential_Drive

Differential drive succeed (Ver. 1.0)

Differential_Drive.cpp@0:644a119c9d8a, 2016-10-26 (annotated)

Committer:
benson516
Date:
Wed Oct 26 17:56:15 2016 +0000
Revision:
0:644a119c9d8a
Differential drive succeed (Ver. 1.0)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:644a119c9d8a 1 // A motion control for differential drive mobile robot
benson516 0:644a119c9d8a 2 #include "Differential_Drive.h"
benson516 0:644a119c9d8a 3
benson516 0:644a119c9d8a 4 using std::abs;
benson516 0:644a119c9d8a 5 using std::vector;
benson516 0:644a119c9d8a 6
benson516 0:644a119c9d8a 7 Diff_Drive::Diff_Drive(float Ts_in, bool Diff_drive_in):
benson516 0:644a119c9d8a 8 Ts(Ts_in),
benson516 0:644a119c9d8a 9 Diff_drive(Diff_drive_in),
benson516 0:644a119c9d8a 10 PID_1(0.0, 0.0, 0.0,Ts_in),
benson516 0:644a119c9d8a 11 PID_2(0.0, 0.0, 0.0,Ts_in)
benson516 0:644a119c9d8a 12 {
benson516 0:644a119c9d8a 13 // Ts = 0.001; // sec., sampling time
benson516 0:644a119c9d8a 14
benson516 0:644a119c9d8a 15 // Ture: differential drive; false: separated control
benson516 0:644a119c9d8a 16 // Diff_drive = false;
benson516 0:644a119c9d8a 17 //
benson516 0:644a119c9d8a 18 if (Diff_drive){
benson516 0:644a119c9d8a 19 // Kp1, Ki1, Kd1, Ka1, Kp2, Ki2, Kd2, Ka2
benson516 0:644a119c9d8a 20 // float K_1[] = {0.0, 0.0, 0.0, 0.0017, 5.73, 11.46, 0.0, 0.0017};
benson516 0:644a119c9d8a 21 // float K_1[] = {5.73, 11.46, 0.0, 0.0017, 0.0, 0.0, 0.0, 0.0017};
benson516 0:644a119c9d8a 22 float K_1[] = {5.73, 11.46, 0.0, 0.0017, 5.73, 11.46, 0.0, 0.0017};
benson516 0:644a119c9d8a 23 K.assign(K_1,K_1+8);
benson516 0:644a119c9d8a 24 }else{
benson516 0:644a119c9d8a 25 // Kp1, Ki1, Kd1, Ka1, Kp2, Ki2, Kd2, Ka2
benson516 0:644a119c9d8a 26 float K_1[] = {5.73, 11.46, 0.0, 0.0017, 5.73, 11.46, 0.0, 0.0017};
benson516 0:644a119c9d8a 27 K.assign(K_1,K_1+8);
benson516 0:644a119c9d8a 28 }
benson516 0:644a119c9d8a 29 //
benson516 0:644a119c9d8a 30
benson516 0:644a119c9d8a 31
benson516 0:644a119c9d8a 32 // Controller parameters
benson516 0:644a119c9d8a 33 // Control gains
benson516 0:644a119c9d8a 34 // K.Size = 8
benson516 0:644a119c9d8a 35 // Kp1, Ki1, Kd1, Ka1, Kp2, Ki2, Kd2, Ka2
benson516 0:644a119c9d8a 36 // float K_1[] = {5.73, 11.46, 0.0, 0.0017, 5.73, 11.46, 0.0, 0.0017};
benson516 0:644a119c9d8a 37 // K.assign(K_1,K_1+8);
benson516 0:644a119c9d8a 38 /*
benson516 0:644a119c9d8a 39 K.resize(8);
benson516 0:644a119c9d8a 40 for (size_t i = 0; i < K.size(); ++i){
benson516 0:644a119c9d8a 41 K[i] = 0.0;
benson516 0:644a119c9d8a 42 }*/
benson516 0:644a119c9d8a 43
benson516 0:644a119c9d8a 44 // Driver parameters
benson516 0:644a119c9d8a 45 // Output limit
benson516 0:644a119c9d8a 46 Vdc = 7.4; // V
benson516 0:644a119c9d8a 47 voltage_limit_H = Vdc; // +Vdc
benson516 0:644a119c9d8a 48 voltage_limit_L = -Vdc; // -Vdc
benson516 0:644a119c9d8a 49 // Input limit
benson516 0:644a119c9d8a 50 W_max = 4.3633; // 250 deg/s = 4.3633 rad/sec.
benson516 0:644a119c9d8a 51 W_max_inv = 1.0/W_max;
benson516 0:644a119c9d8a 52
benson516 0:644a119c9d8a 53 // Motor parameters
benson516 0:644a119c9d8a 54 // Ke = 1.6748; // Speed constant, volt.-sec./rad <- This may be repalced by a nonlinear function f_e(w)
benson516 0:644a119c9d8a 55 Ke = 1.6;
benson516 0:644a119c9d8a 56 Kt = Ke; // Torque Constant, Nt./Amp. (Kt almost equals to Ke, Ke >=Kt)
benson516 0:644a119c9d8a 57
benson516 0:644a119c9d8a 58 // Robot parameters
benson516 0:644a119c9d8a 59 b = 0.1; // m, half of robot's wheel axle
benson516 0:644a119c9d8a 60 r = 0.045; // m, wheel radious
benson516 0:644a119c9d8a 61 //
benson516 0:644a119c9d8a 62 b_inv = 1.0/b;
benson516 0:644a119c9d8a 63 r_half = r*0.5;
benson516 0:644a119c9d8a 64 r_inv = 1.0/r;
benson516 0:644a119c9d8a 65 r_half_b_inv = r_half*b_inv; // r_half*b_inv
benson516 0:644a119c9d8a 66
benson516 0:644a119c9d8a 67 // States(feedback signals), commands, outputs,
benson516 0:644a119c9d8a 68 // States(feedback signals)
benson516 0:644a119c9d8a 69 V_W.resize(2,0.0); // Speed-rotaion domain
benson516 0:644a119c9d8a 70 W1_W2.resize(2,0.0); // Two-single-wheel domain, W1: right wheel, W2: left wheel
benson516 0:644a119c9d8a 71 // Commands
benson516 0:644a119c9d8a 72 Vd_Wd.resize(2, 0.0); // Input from higher-level commander
benson516 0:644a119c9d8a 73 W1d_W2d.resize(2, 0.0);
benson516 0:644a119c9d8a 74 W1dS_W2dS.resize(2, 0.0); // Saturated command for W1 and W2
benson516 0:644a119c9d8a 75 VdS_WdS.resize(2, 0.0); // Saturated command for V and W
benson516 0:644a119c9d8a 76 // Outputs (voltage command)
benson516 0:644a119c9d8a 77 UV_UW.resize(2, 0.0); // Controller output, speed-rotaion domain
benson516 0:644a119c9d8a 78 U1_U2.resize(2, 0.0); // Controller output, two-single-wheel domain, U1: right wheel, U2: left wheel
benson516 0:644a119c9d8a 79 V1_V2.resize(2, 0.0); // Voltage compensated with back-emf, two-single-wheel domain, V1: right wheel, V2: left wheel
benson516 0:644a119c9d8a 80 V1S_V2S.resize(2, 0.0); // Saturated voltage, two-single-wheel domain
benson516 0:644a119c9d8a 81
benson516 0:644a119c9d8a 82 delta_V1_V2.resize(2, 0.0); // The difference between saturated and original voltage command
benson516 0:644a119c9d8a 83 // deltaVi = VSi - Vi
benson516 0:644a119c9d8a 84 delta_VV_VW.resize(2,0.0); // Speed-rotation domain
benson516 0:644a119c9d8a 85
benson516 0:644a119c9d8a 86
benson516 0:644a119c9d8a 87 // Set PID's parameters
benson516 0:644a119c9d8a 88 /////////////////////
benson516 0:644a119c9d8a 89 PID_1.Kp = K[0];
benson516 0:644a119c9d8a 90 PID_1.Ki = K[1];
benson516 0:644a119c9d8a 91 PID_1.Kd = K[2];
benson516 0:644a119c9d8a 92 PID_1.Ka = K[3]; // Gain for anti-windup
benson516 0:644a119c9d8a 93 //
benson516 0:644a119c9d8a 94 PID_2.Kp = K[4];
benson516 0:644a119c9d8a 95 PID_2.Ki = K[5];
benson516 0:644a119c9d8a 96 PID_2.Kd = K[6];
benson516 0:644a119c9d8a 97 PID_2.Ka = K[7]; // Gain for anti-windup
benson516 0:644a119c9d8a 98
benson516 0:644a119c9d8a 99 }
benson516 0:644a119c9d8a 100 // T: two-single-wheel domain |-> speed-rotaion domain
benson516 0:644a119c9d8a 101 //////////////////////
benson516 0:644a119c9d8a 102 void Diff_Drive::Transform(const vector<float> &V_in, vector<float> &V_out){
benson516 0:644a119c9d8a 103 V_out[0] = r_half*(V_in[0] + V_in[1]);
benson516 0:644a119c9d8a 104 V_out[1] = r_half_b_inv*(V_in[0] - V_in[1]);
benson516 0:644a119c9d8a 105 }
benson516 0:644a119c9d8a 106 void Diff_Drive::Transform_inv(const vector<float> &V_in, vector<float> &V_out){
benson516 0:644a119c9d8a 107 V_out[0] = r_inv*(V_in[0] + b*V_in[1]);
benson516 0:644a119c9d8a 108 V_out[1] = r_inv*(V_in[0] - b*V_in[1]);
benson516 0:644a119c9d8a 109 }
benson516 0:644a119c9d8a 110 ////////////////////// end T
benson516 0:644a119c9d8a 111
benson516 0:644a119c9d8a 112 // Main process
benson516 0:644a119c9d8a 113 ////////////////////
benson516 0:644a119c9d8a 114 void Diff_Drive::compute(float Vd, float Wd, float W1, float W2){
benson516 0:644a119c9d8a 115 if (Diff_drive)
benson516 0:644a119c9d8a 116 compute_DiffDriveMethod(Vd, Wd, W1, W2);
benson516 0:644a119c9d8a 117 else
benson516 0:644a119c9d8a 118 compute_SeparatedMethod(Vd, Wd, W1, W2);
benson516 0:644a119c9d8a 119
benson516 0:644a119c9d8a 120 }
benson516 0:644a119c9d8a 121 void Diff_Drive::compute_SeparatedMethod(float Vd, float Wd, float W1, float W2){
benson516 0:644a119c9d8a 122
benson516 0:644a119c9d8a 123 Vd_Wd[0] = Vd;
benson516 0:644a119c9d8a 124 Vd_Wd[1] = Wd;
benson516 0:644a119c9d8a 125
benson516 0:644a119c9d8a 126 Transform_inv(Vd_Wd,W1d_W2d);
benson516 0:644a119c9d8a 127 // Input saturation
benson516 0:644a119c9d8a 128 // W1dS_W2dS = W1d_W2d; // <- This should be replaced by input saturation
benson516 0:644a119c9d8a 129 Saturation_input(W1d_W2d, W1dS_W2dS, true);
benson516 0:644a119c9d8a 130
benson516 0:644a119c9d8a 131 // Feedback in
benson516 0:644a119c9d8a 132 W1_W2[0] = W1;
benson516 0:644a119c9d8a 133 W1_W2[1] = W2;
benson516 0:644a119c9d8a 134
benson516 0:644a119c9d8a 135 // PI
benson516 0:644a119c9d8a 136 PID_1.Compute_noWindUP(W1dS_W2dS[0], W1_W2[0]);
benson516 0:644a119c9d8a 137 PID_2.Compute_noWindUP(W1dS_W2dS[1], W1_W2[1]);
benson516 0:644a119c9d8a 138 U1_U2[0] = PID_1.output;
benson516 0:644a119c9d8a 139 U1_U2[1] = PID_2.output;
benson516 0:644a119c9d8a 140
benson516 0:644a119c9d8a 141 // Back emf compensation
benson516 0:644a119c9d8a 142 V1_V2[0] = U1_U2[0] + func_back_emf(W1_W2[0]);
benson516 0:644a119c9d8a 143 V1_V2[1] = U1_U2[1] + func_back_emf(W1_W2[1]);
benson516 0:644a119c9d8a 144
benson516 0:644a119c9d8a 145 //Output saturation
benson516 0:644a119c9d8a 146 Saturation_output(V1_V2, V1S_V2S, delta_V1_V2, true);
benson516 0:644a119c9d8a 147
benson516 0:644a119c9d8a 148 // Anti-windup compensation
benson516 0:644a119c9d8a 149 PID_1.Anti_windup(delta_V1_V2[0]);
benson516 0:644a119c9d8a 150 PID_2.Anti_windup(delta_V1_V2[1]);
benson516 0:644a119c9d8a 151 //
benson516 0:644a119c9d8a 152
benson516 0:644a119c9d8a 153
benson516 0:644a119c9d8a 154 }
benson516 0:644a119c9d8a 155 void Diff_Drive::compute_DiffDriveMethod(float Vd, float Wd, float W1, float W2){
benson516 0:644a119c9d8a 156
benson516 0:644a119c9d8a 157 Vd_Wd[0] = Vd;
benson516 0:644a119c9d8a 158 Vd_Wd[1] = Wd;
benson516 0:644a119c9d8a 159
benson516 0:644a119c9d8a 160 Transform_inv(Vd_Wd,W1d_W2d);
benson516 0:644a119c9d8a 161 // Input saturation
benson516 0:644a119c9d8a 162 // W1dS_W2dS = W1d_W2d; // <- This should be replaced by input saturation
benson516 0:644a119c9d8a 163 Saturation_input(W1d_W2d, W1dS_W2dS, true);
benson516 0:644a119c9d8a 164
benson516 0:644a119c9d8a 165 // Feedback in
benson516 0:644a119c9d8a 166 W1_W2[0] = W1;
benson516 0:644a119c9d8a 167 W1_W2[1] = W2;
benson516 0:644a119c9d8a 168
benson516 0:644a119c9d8a 169 // Speed-rotation domain
benson516 0:644a119c9d8a 170 /////////////////////////
benson516 0:644a119c9d8a 171 Transform(W1dS_W2dS, VdS_WdS);
benson516 0:644a119c9d8a 172 Transform(W1_W2, V_W);
benson516 0:644a119c9d8a 173
benson516 0:644a119c9d8a 174 // PI
benson516 0:644a119c9d8a 175 PID_1.Compute_noWindUP(VdS_WdS[0], V_W[0]);
benson516 0:644a119c9d8a 176 PID_2.Compute_noWindUP(VdS_WdS[1], V_W[1]);
benson516 0:644a119c9d8a 177 UV_UW[0] = PID_1.output;
benson516 0:644a119c9d8a 178 UV_UW[1] = PID_2.output;
benson516 0:644a119c9d8a 179
benson516 0:644a119c9d8a 180 //
benson516 0:644a119c9d8a 181 Transform_inv(UV_UW, U1_U2);
benson516 0:644a119c9d8a 182 ///////////////////////// end Speed-rotation domain
benson516 0:644a119c9d8a 183
benson516 0:644a119c9d8a 184 // Back emf compensation
benson516 0:644a119c9d8a 185 V1_V2[0] = U1_U2[0] + func_back_emf(W1_W2[0]);
benson516 0:644a119c9d8a 186 V1_V2[1] = U1_U2[1] + func_back_emf(W1_W2[1]);
benson516 0:644a119c9d8a 187
benson516 0:644a119c9d8a 188 //Output saturation
benson516 0:644a119c9d8a 189 Saturation_output(V1_V2, V1S_V2S, delta_V1_V2, true);
benson516 0:644a119c9d8a 190
benson516 0:644a119c9d8a 191
benson516 0:644a119c9d8a 192 // Speed-rotation domain
benson516 0:644a119c9d8a 193 ////////////////////////////////
benson516 0:644a119c9d8a 194 Transform(delta_V1_V2, delta_VV_VW);
benson516 0:644a119c9d8a 195
benson516 0:644a119c9d8a 196 // Anti-windup compensation
benson516 0:644a119c9d8a 197 PID_1.Anti_windup(delta_VV_VW[0]);
benson516 0:644a119c9d8a 198 PID_2.Anti_windup(delta_VV_VW[1]);
benson516 0:644a119c9d8a 199 //
benson516 0:644a119c9d8a 200 //////////////////////////////// end Speed-rotation domain
benson516 0:644a119c9d8a 201 }
benson516 0:644a119c9d8a 202 //////////////////// end Main process
benson516 0:644a119c9d8a 203
benson516 0:644a119c9d8a 204
benson516 0:644a119c9d8a 205 // Get results
benson516 0:644a119c9d8a 206 float Diff_Drive::get_V1S(){
benson516 0:644a119c9d8a 207 // Saturated output
benson516 0:644a119c9d8a 208 return V1S_V2S[0];
benson516 0:644a119c9d8a 209 }
benson516 0:644a119c9d8a 210 float Diff_Drive::get_V2S(){
benson516 0:644a119c9d8a 211 // Saturated output
benson516 0:644a119c9d8a 212 return V1S_V2S[1];
benson516 0:644a119c9d8a 213 }
benson516 0:644a119c9d8a 214
benson516 0:644a119c9d8a 215 // Saturation
benson516 0:644a119c9d8a 216 void Diff_Drive::Saturation_input(const vector<float> &in, vector<float> &out, bool enable){
benson516 0:644a119c9d8a 217 if (enable){
benson516 0:644a119c9d8a 218 // out = in;
benson516 0:644a119c9d8a 219 float alpha_1 = abs(in[0])*W_max_inv;
benson516 0:644a119c9d8a 220 float alpha_2 = abs(in[1])*W_max_inv;
benson516 0:644a119c9d8a 221 float alpha_inv = 0.0;
benson516 0:644a119c9d8a 222 //
benson516 0:644a119c9d8a 223 if ((alpha_1 > 1) && (alpha_2 > 1)){
benson516 0:644a119c9d8a 224 if (alpha_1 >= alpha_2){
benson516 0:644a119c9d8a 225 alpha_inv = 1/alpha_1;
benson516 0:644a119c9d8a 226 }else{
benson516 0:644a119c9d8a 227 alpha_inv = 1/alpha_2;
benson516 0:644a119c9d8a 228 }
benson516 0:644a119c9d8a 229 //
benson516 0:644a119c9d8a 230 out[0] = alpha_inv*in[0];
benson516 0:644a119c9d8a 231 out[1] = alpha_inv*in[1];
benson516 0:644a119c9d8a 232 }else if (alpha_1 > 1){
benson516 0:644a119c9d8a 233 alpha_inv = 1/alpha_1;
benson516 0:644a119c9d8a 234 //
benson516 0:644a119c9d8a 235 out[0] = alpha_inv*in[0];
benson516 0:644a119c9d8a 236 out[1] = alpha_inv*in[1];
benson516 0:644a119c9d8a 237 }else if (alpha_2 > 1){
benson516 0:644a119c9d8a 238 alpha_inv = 1/alpha_2;
benson516 0:644a119c9d8a 239 //
benson516 0:644a119c9d8a 240 out[0] = alpha_inv*in[0];
benson516 0:644a119c9d8a 241 out[1] = alpha_inv*in[1];
benson516 0:644a119c9d8a 242 }else{
benson516 0:644a119c9d8a 243 // nothing saturated
benson516 0:644a119c9d8a 244 out = in;
benson516 0:644a119c9d8a 245 }
benson516 0:644a119c9d8a 246 }else{
benson516 0:644a119c9d8a 247 out = in;
benson516 0:644a119c9d8a 248 }
benson516 0:644a119c9d8a 249 }
benson516 0:644a119c9d8a 250 void Diff_Drive::Saturation_output(const vector<float> &in, vector<float> &out, vector<float> &delta_out, bool enable){
benson516 0:644a119c9d8a 251 if (enable){
benson516 0:644a119c9d8a 252 // Output 1
benson516 0:644a119c9d8a 253 if (in[0] > voltage_limit_H){
benson516 0:644a119c9d8a 254 delta_out[0] = voltage_limit_H - in[0];
benson516 0:644a119c9d8a 255 out[0] = voltage_limit_H;
benson516 0:644a119c9d8a 256 }else if (in[0] < voltage_limit_L){
benson516 0:644a119c9d8a 257 delta_out[0] = voltage_limit_L - in[0];
benson516 0:644a119c9d8a 258 out[0] = voltage_limit_L;
benson516 0:644a119c9d8a 259 }else{
benson516 0:644a119c9d8a 260 out[0] = in[0];
benson516 0:644a119c9d8a 261 //
benson516 0:644a119c9d8a 262 delta_out[0] = 0.0;
benson516 0:644a119c9d8a 263 }
benson516 0:644a119c9d8a 264 // Output 2
benson516 0:644a119c9d8a 265 if (in[1] > voltage_limit_H){
benson516 0:644a119c9d8a 266 delta_out[1] = voltage_limit_H - in[1];
benson516 0:644a119c9d8a 267 out[1] = voltage_limit_H;
benson516 0:644a119c9d8a 268 }else if (in[1] < voltage_limit_L){
benson516 0:644a119c9d8a 269 delta_out[1] = voltage_limit_L - in[1];
benson516 0:644a119c9d8a 270 out[1] = voltage_limit_L;
benson516 0:644a119c9d8a 271 }else{
benson516 0:644a119c9d8a 272 out[1] = in[1];
benson516 0:644a119c9d8a 273 //
benson516 0:644a119c9d8a 274 delta_out[1] = 0.0;
benson516 0:644a119c9d8a 275 }
benson516 0:644a119c9d8a 276 }else{
benson516 0:644a119c9d8a 277 out = in;
benson516 0:644a119c9d8a 278 //
benson516 0:644a119c9d8a 279 delta_out[0] = 0.0;
benson516 0:644a119c9d8a 280 delta_out[1] = 0.0;
benson516 0:644a119c9d8a 281 }
benson516 0:644a119c9d8a 282 }
benson516 0:644a119c9d8a 283
benson516 0:644a119c9d8a 284 // Back emf as the function of rotational speed
benson516 0:644a119c9d8a 285 float Diff_Drive::func_back_emf(float W_in){
benson516 0:644a119c9d8a 286 return (Ke*W_in);
benson516 0:644a119c9d8a 287 }