mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-24 03:02:45 +00:00
0812db21ef
- Fix missing references to `obj` - Fix `npredinputs` so it matches `npredstates` as specified by `forwardIntegrate.m` - Fix typo: `get_start_idx` -> `obj.get_state_start_idx` - Fix nonlinear constraint indexing issue - Fix nonlinear constraint obstacle indexing typo - Remove unused `nonlinear_bike_model` function - Compute continuous system matrices offline and convert to function with `matlabFunction`: `A_c_func` & `B_c_func` - Add `miscellaneous_stuff.m` which has some random testing and continuous system matrix generation code - Add `part2_test_controller.m` for testing part2 deliverable
68 lines
2.4 KiB
Matlab
68 lines
2.4 KiB
Matlab
%% Part 1 Testing Xenia's Code
|
|
close all;
|
|
figure(1)
|
|
hold on
|
|
plot(Y_ref(1,:), Y_ref(3,:), 'g-')
|
|
plot(Y(1,:), Y(3,:), 'b-')
|
|
|
|
[Y_test, T_test] = forwardIntegrateControlInput(U', init);
|
|
traj_info = getTrajectoryInfo(Y_test(1:439,:), U(:,1:439)')
|
|
plot(Y_test(:,1), Y_test(:,3), 'k-')
|
|
|
|
plot(TestTrack.bl(1,:), TestTrack.bl(2,:), 'r.')
|
|
plot(TestTrack.br(1,:), TestTrack.br(2,:), 'r.')
|
|
|
|
%% Part 2 Symbolic Jacobians
|
|
close all;
|
|
clear all;
|
|
clc;
|
|
|
|
Nw = 2;
|
|
f = 0.01;
|
|
Iz = 2667;
|
|
a = 1.35;
|
|
b = 1.45;
|
|
By = 0.27;
|
|
Cy = 1.2;
|
|
Dy = 0.7;
|
|
Ey = -1.6;
|
|
Shy = 0;
|
|
Svy = 0;
|
|
m = 1400;
|
|
g = 9.806;
|
|
|
|
syms x_var u_var y_var v_var psi_var r_var ... % states
|
|
delta_f_var F_x_var ... % inputs
|
|
F_yf_var F_yr_var % lateral forces
|
|
|
|
% Vehicle Dynamics (Equation 1)
|
|
dx = u_var*cos(psi_var) - v_var*sin(psi_var);
|
|
du = (1/m)*(-f*m*g + Nw*F_x_var - F_yf_var*sin(delta_f_var)) + v_var*r_var;
|
|
dy = u_var*sin(psi_var) + v_var*cos(psi_var);
|
|
dv = (1/m)*(F_yf_var*cos(delta_f_var) + F_yr_var) - u_var*r_var;
|
|
dpsi = r_var;
|
|
dr = (1/Iz)*(a*F_yf_var*cos(delta_f_var) - b*F_yr_var);
|
|
|
|
% Jacobians of continuous-time linearized system
|
|
% TODO: Change A_c & B_c computation to regular functions (e.g.,
|
|
% using `matlabFunction`) if speed is an issue
|
|
A_c_symb = [ ...
|
|
diff(dx, x_var), diff(dx, u_var), diff(dx, y_var), diff(dx, v_var), diff(dx, psi_var), diff(dx, r_var);
|
|
diff(du, x_var), diff(du, u_var), diff(du, y_var), diff(du, v_var), diff(du, psi_var), diff(du, r_var);
|
|
diff(dy, x_var), diff(dy, u_var), diff(dy, y_var), diff(dy, v_var), diff(dy, psi_var), diff(dy, r_var);
|
|
diff(dv, x_var), diff(dv, u_var), diff(dv, y_var), diff(dv, v_var), diff(dv, psi_var), diff(dv, r_var);
|
|
diff(dpsi,x_var), diff(dpsi,u_var), diff(dpsi,y_var), diff(dpsi,v_var), diff(dpsi,psi_var), diff(dpsi,r_var);
|
|
diff(dr, x_var), diff(dr, u_var), diff(dr, y_var), diff(dr, v_var), diff(dr, psi_var), diff(dr, r_var);
|
|
];
|
|
|
|
B_c_symb = [ ...
|
|
diff(dx, delta_f_var), diff(dx, F_x_var);
|
|
diff(du, delta_f_var), diff(du, F_x_var);
|
|
diff(dy, delta_f_var), diff(dy, F_x_var);
|
|
diff(dv, delta_f_var), diff(dv, F_x_var);
|
|
diff(dpsi,delta_f_var), diff(dpsi,F_x_var);
|
|
diff(dr, delta_f_var), diff(dr, F_x_var);
|
|
];
|
|
|
|
A_c = matlabFunction(A_c_symb, 'File', 'A_c_func');
|
|
B_c = matlabFunction(B_c_symb, 'File', 'B_c_func'); |