mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-21 02:02:44 +00:00
69c704e66b
- Decrease `sim_stop_idx` to 1500 - Add some debugging messages for when simulation hits limit or when `fmincon` can't satisfy constraints - Add more `fmincon` outputs for future debugging - Add obstacle information to `getTrajectoryInfo` call in `part2_test_controller.m` to see when car crashes
31 lines
753 B
Matlab
31 lines
753 B
Matlab
%% Close Figures, Clear Workspace, and Clear Terminal
|
|
close all;
|
|
clear;
|
|
clc;
|
|
|
|
%% Call simulation functions
|
|
[Y,U,t_total,t_update,Xobs] = forwardIntegrate();
|
|
info = getTrajectoryInfo(Y,U,Xobs)
|
|
|
|
%% Figures
|
|
% Plot segmented trajectory for debugging purposes
|
|
close all;
|
|
figure(1)
|
|
hold on;
|
|
grid on;
|
|
|
|
load('TestTrack.mat')
|
|
load('ROB535_ControlProject_part1_Team3.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.-');
|
|
|
|
for i = 1:length(Xobs)
|
|
xvals = Xobs{i}(:,1);
|
|
yvals = Xobs{i}(:,2);
|
|
fill(xvals, yvals, 'm');
|
|
end
|
|
|
|
plot(ROB535_ControlProject_part1_state(:,1), ROB535_ControlProject_part1_state(:,3), 'k--');
|
|
plot(Y(:,1), Y(:,3), 'b-'); |