Simulation Stop Index & Obstacle Plotting

- Add parameter to stop simulation when specified
  index in reference trajectory is reached
- Modify `forwardIntegrate` to return `Xobs` for plotting
- Plot obstacles in part2 testing figure
- Change some line plotting styles for clarity

Deliverables/MPC_Class.m

@@ -56,6 +56,7 @@ classdef MPC_Class
         % Simulation Parameters
         T_s = 0.01;         % Step Size [s]
         T_p = 0.5;          % Prediction Horizon [s]
+        sim_stop_idx = 2e3; % Index in reference trajectory to stop sim
 
         % Decision Variables
         nstates = 6; % Number of states per prediction
@@ -126,10 +127,9 @@ classdef MPC_Class
             obj.ndecinputs  = obj.npredinputs * obj.ninputs;
             obj.ndec        = obj.ndecstates + obj.ndecinputs;
 
-            % Check if `Y_curr` is within prediction horizon of
+            % Stop simulation when `sim_stop_idx` is reached
-            % the end of `Y_ref`
             obj.FLAG_terminate = 0;
-            if size(obj.Y_ref, 1) - obj.ref_idx < obj.npred
+            if obj.ref_idx > obj.sim_stop_idx
                 obj.FLAG_terminate = 1;
             end
         end

Experimentation/part2_test_controller.m

@@ -4,7 +4,7 @@ clear;
 clc;
 
 %% Call simulation functions
-[Y,U,t_total,t_update] = forwardIntegrate();
+[Y,U,t_total,t_update,Xobs] = forwardIntegrate();
 info = getTrajectoryInfo(Y,U)
 
 %% Figures
@@ -17,9 +17,15 @@ grid on;
 load('TestTrack.mat')
 load('ROB535_ControlProject_part1_Team3.mat')
 
-plot(Y(:,1), Y(:,3), 'b-');
-plot(ROB535_ControlProject_part1_state(:,1), ROB535_ControlProject_part1_state(:,3), 'k-');
-
 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-');

Simulation/forwardIntegrate.m

@@ -1,4 +1,4 @@
-function [Y,U,t_total,t_update] = forwardIntegrate()
+function [Y,U,t_total,t_update,Xobs] = forwardIntegrate()
 % [Y,U,t_total,t_update] = forwardIntegrate
 % 
 % This script returns the vehicle trajectory with control input being