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

Plot Trajectory of Submission Inputs

Browse Source 
- Load the mat file with part 1 deliverable inputs
- Plot second figure with final submission trajectory
This commit is contained in:
Sravan Balaji
2021-11-26 15:48:04 -05:00
parent 6cde3df187
commit 420dc67a26
1 changed files with 17 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
part1_generate_trajectory.m
View File

@@ -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