Car testdrive (lane change manoeuvre) (Muscod)
From mintOC
Revision as of 08:47, 28 June 2016 by FelixMueller (Talk | contribs) (Created page with "This page contains the Muscod code for the Car testdrive (lane change manoeuvre) problem. The differential equations in C code: <source lang="cpp"...")
This page contains the Muscod code for the Car testdrive (lane change manoeuvre) problem.
The differential equations in C code:
// Controls double C_steer = u[0]; double C_brake = u[1]; double C_acc = u[2]; // Differential states double X_v = xd[2]; double X_beta = xd[3]; double X_psi = xd[4]; double X_wz = xd[5]; double X_delta = xd[6]; // Intermediate values double alpha_f, alpha_r, v_km_h, v_km_h2; double F_Ax, F_Ay, F_Bf, F_Br, F_Rf, F_Rr, F_sf, F_sr, F_lr, F_lf; double f_R, f_1, w_mot, f_2, f_3, M_mot, M_wheel; double X_v_cos_X_beta, X_v_sin_X_beta; X_v_cos_X_beta = X_v * cos ( X_beta ); X_v_sin_X_beta = X_v * sin ( X_beta ); alpha_f = X_delta - atan( ( P_l_f * X_wz - X_v_sin_X_beta ) / X_v_cos_X_beta ); alpha_r = atan( ( P_l_r * X_wz + X_v_sin_X_beta ) / X_v_cos_X_beta ); F_sf = P_D_f * sin( P_C_f * atan( P_B_f*alpha_f - P_E_f*(P_B_f*alpha_f - atan(P_B_f*alpha_f)) ) ); F_sr = P_D_r * sin( P_C_r * atan( P_B_r*alpha_r - P_E_r*(P_B_r*alpha_r - atan(P_B_r*alpha_r)) ) ); F_Ax = 0.5 * P_c_w * P_rho * P_A * X_v*X_v; F_Ay = 0.0; F_Bf = 2.0/3.0 * C_brake; F_Br = 1.0/3.0 * C_brake; v_km_h = X_v / 100.0 * 3.6; v_km_h2 = v_km_h * v_km_h; f_R = P_f_R0 + P_f_R1 * v_km_h + P_f_R4 * v_km_h2 * v_km_h2; F_Rf = f_R * P_F_zf; F_Rr = f_R * P_F_zr; f_1 = 1.0 - exp( -3.0 * C_acc ); w_mot = X_v * P_ig * P_i_t / P_R; f_2 = -37.8 + (1.54 - 0.0019 * w_mot_i) * w_mot; f_3 = -34.9 - 0.04775 * w_mot; M_mot = f_1 * f_2_i + ( 1.0 - f_1 ) * f_3_i; M_wheel = P_ig * P_i_t * M_mot_i; F_lr = M_wheel / P_R - F_Br - F_Rr; F_lf = - F_Bf - F_Rf; // 0 Horizontal position x rhs[0] = X_v * cos( X_psi - X_beta ); // 1 Vertical position y rhs[1] = X_v * sin( X_psi - X_beta ); // 2 Velocity v rhs[2] = 1.0 / P_m * ( (F_lr - F_Ax) * cos(X_beta) + F_lf * cos(X_delta + X_beta) - (F_sr - F_Ay) * sin(X_beta) - F_sf * sin(X_delta + X_beta) ); // 3 Side slip angle beta rhs[3] = X_wz - 1.0 / (P_m * X_v) * ( (F_lr - F_Ax) * sin(X_beta) + F_lf * sin(X_delta + X_beta) + (F_sr - F_Ay) * cos(X_beta) + F_sf * cos(X_delta + X_beta) ); // 4 Yaw angle psi rhs[4] = X_wz; // 5 Velocity of yaw angle w_z rhs[5] = 1.0 / P_I_zz * ( F_sf * P_l_f * cos(X_delta) - F_sr * P_l_r + F_lf * P_l_f * sin(X_delta) - F_Ay * P_e_SP ); // 6 Steering angle delta rhs[6] = C_steer;