sravan/Control-Project
1
0
Fork 0
mirror of https://github.com/ROB-535-F21-Team-3/Control-Project.git synced 2025-08-27 04:12:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

Bunch of Fixes & Add Some Testing Scripts

Browse Source 
- 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
This commit is contained in:
Sravan Balaji
2021-12-12 15:27:37 -05:00
parent 6361cfd40c
commit 0812db21ef
3 changed files with 156 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
Experimentation/miscellaneous_stuff.m Normal file
View File

@@ -0,0 +1,68 @@
%% 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');