Bunch of Fixes & Add Some Testing Scripts

- 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

View File

@@ -0,0 +1,21 @@
%% Close Figures, Clear Workspace, and Clear Terminal
close all;
clear;
clc;
%% Call simulation functions
[Y,U,t_total,t_update] = forwardIntegrate();
info = getTrajectoryInfo(Y,U)
%% Figures
% Plot segmented trajectory for debugging purposes
figure(1)
hold on;
grid on;
plot(Y(:,1), Y(:,3), '-');
load('TestTrack.mat')
plot(TestTrack.bl(1,:), TestTrack.bl(2,:), '--r');
plot(TestTrack.br(1,:), TestTrack.br(2,:), '--r');
plot(TestTrack.cline(1,:), TestTrack.cline(2,:), '-.g');