From 420dc67a265b21eff83e78d787a8ac0e9f300b2c Mon Sep 17 00:00:00 2001
From: Sravan Balaji <balajsra@umich.edu>
Date: Fri, 26 Nov 2021 15:48:04 -0500
Subject: [PATCH] Plot Trajectory of Submission Inputs

- Load the mat file with part 1 deliverable inputs
- Plot second figure with final submission trajectory
---
 part1_generate_trajectory.m | 19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/part1_generate_trajectory.m b/part1_generate_trajectory.m
index 07f7614..e330ecd 100644
--- a/part1_generate_trajectory.m
+++ b/part1_generate_trajectory.m
@@ -166,6 +166,9 @@ num_pts(segment_num) = 9e2;
 delta_vals(segment_num) = 0.0;
 F_x_vals(segment_num) = 5000;
 
+%% Load Inputs from File
+load('ROB535_ControlProject_part1_Team3.mat');
+
 %% Simulate Trajectory
 for i = 1:length(num_pts)
     [start_idx, end_idx] = get_indices(i, num_pts);
@@ -178,9 +181,11 @@ end
 [Y, T] = forwardIntegrateControlInput(U, state_0);
 info = getTrajectoryInfo(Y,U)
 
-% Figures
-close all;
+[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, state_0);
+info = getTrajectoryInfo(Y_submission,ROB535_ControlProject_part1_input)
 
+%% Figures
+% Plot segmented trajectory for debugging purposes
 figure(1)
 hold on;
 grid on;
@@ -194,6 +199,16 @@ plot(TestTrack.bl(1,:), TestTrack.bl(2,:), '--r');
 plot(TestTrack.br(1,:), TestTrack.br(2,:), '--r');
 plot(TestTrack.cline(1,:), TestTrack.cline(2,:), '-.g');
 
+% Plot final trajectory from submission inputs
+figure(2)
+hold on;
+grid on;
+
+plot(Y_submission(:,1), Y_submission(:,3), '-b');
+plot(TestTrack.bl(1,:), TestTrack.bl(2,:), '-r');
+plot(TestTrack.br(1,:), TestTrack.br(2,:), '-r');
+plot(TestTrack.cline(1,:), TestTrack.cline(2,:), '--g');
+
 %% Functions
 function [start_idx, end_idx] = get_indices(segment_num, num_pts)
     if segment_num == 1