sravan/Control-Project
1
0
Fork 0
mirror of https://github.com/ROB-535-F21-Team-3/Control-Project.git synced 2025-09-02 22:33:15 +00:00
Code Issues Packages Projects Releases Wiki Activity

just committing bc git confuses me

Browse Source
This commit is contained in:
xdemenchuk
2021-12-10 10:49:26 -05:00
parent 4165d10a72
commit 01735c90d7
2 changed files with 21 additions and 281 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
SimPkg_F21(student_ver)/part1_generate_trajectory.m
View File

@@ -184,6 +184,27 @@ info = getTrajectoryInfo(Y,U)
[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, state_0);
info = getTrajectoryInfo(Y_submission,ROB535_ControlProject_part1_input)
index_cl_cp = 1;
index_cl_nextcp = 2;
%xenia test
for i = 1:size(Y,1)
curr_xy = [Y(i,1); Y(i,3)];
dist2cp1 = norm(curr_xy - TestTrack.cline(:, index_cl_cp));
dist2cp2 = norm(curr_xy - TestTrack.cline(:, index_cl_nextcp));
if (dist2cp2 < dist2cp1)
i
dist2cp1
dist2cp2
index_cl_cp = index_cl_cp + 1;
index_cl_nextcp = index_cl_nextcp + 1;
if (index_cl_nextcp > size(TestTrack.cline, 2))
break
end
end
end
%% Figures
% Plot segmented trajectory for debugging purposes
figure(1)