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

Create New Trajectory

Browse Source
This commit is contained in:
Sravan Balaji
2021-12-13 10:25:18 -05:00
parent d591bb68c9
commit 7d502fa8f5
2 changed files with 81 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

144
Experimentation/part1_generate_trajectory.m
View File

@@ -36,135 +36,153 @@ load('TestTrack.mat')
%% Trajectory Synthesis
% Segment 1
segment_num = 1;
num_pts(segment_num) = 6e2;
delta_vals(segment_num) = -0.004;
F_x_vals(segment_num) = 3900;
num_pts(segment_num) = 3e2;
delta_vals(segment_num) = -0.010;
F_x_vals(segment_num) = 2500;
% Segment 2
segment_num = 2;
num_pts(segment_num) = 4e2;
delta_vals(segment_num) = -0.3;
F_x_vals(segment_num) = -2000;
delta_vals(segment_num) = 0.001;
F_x_vals(segment_num) = 2500;
% Segment 3
segment_num = 3;
num_pts(segment_num) = 1e2;
delta_vals(segment_num) = -0.05;
num_pts(segment_num) = 5.5e2;
delta_vals(segment_num) = -0.0255;
F_x_vals(segment_num) = 0;
% Segment 4
segment_num = 4;
num_pts(segment_num) = 7.5e2;
delta_vals(segment_num) = 0.0;
F_x_vals(segment_num) = 1000;
num_pts(segment_num) = 1e2;
delta_vals(segment_num) = 0.017;
F_x_vals(segment_num) = 0;
% Segment 5
segment_num = 5;
num_pts(segment_num) = 3e2;
delta_vals(segment_num) = 0.3;
F_x_vals(segment_num) = -500;
delta_vals(segment_num) = -0.002;
F_x_vals(segment_num) = 2500;
% Segment 6
segment_num = 6;
num_pts(segment_num) = 3.5e2;
delta_vals(segment_num) = -0.03;
F_x_vals(segment_num) = 1000;
delta_vals(segment_num) = 0.0175;
F_x_vals(segment_num) = -800;
% Segment 7
segment_num = 7;
num_pts(segment_num) = 1e2;
delta_vals(segment_num) = -0.005;
F_x_vals(segment_num) = -1000;
num_pts(segment_num) = 2e2;
delta_vals(segment_num) = -0.025;
F_x_vals(segment_num) = 0;
% Segment 8
segment_num = 8;
num_pts(segment_num) = 2e2;
delta_vals(segment_num) = 0.0275;
F_x_vals(segment_num) = -750;
num_pts(segment_num) = 1.85e2;
delta_vals(segment_num) = -0.025;
F_x_vals(segment_num) = -2000;
% Segment 9
segment_num = 9;
num_pts(segment_num) = 2.4e2;
delta_vals(segment_num) = 0.5;
num_pts(segment_num) = 3e2;
delta_vals(segment_num) = 0.04;
F_x_vals(segment_num) = -500;
% Segment 10
segment_num = 10;
num_pts(segment_num) = 5e2;
delta_vals(segment_num) = -0.02;
num_pts(segment_num) = 3e2;
delta_vals(segment_num) = 0.0175;
F_x_vals(segment_num) = 0;
% Segment 11
segment_num = 11;
num_pts(segment_num) = 2.5e2;
delta_vals(segment_num) = -0.05;
F_x_vals(segment_num) = 500;
num_pts(segment_num) = 5.5e2;
delta_vals(segment_num) = -0.03;
F_x_vals(segment_num) = 0;
% Segment 12
segment_num = 12;
num_pts(segment_num) = 2e2;
delta_vals(segment_num) = -0.01;
F_x_vals(segment_num) = 5000;
num_pts(segment_num) = 3e2;
delta_vals(segment_num) = -0.02;
F_x_vals(segment_num) = 0;
% Segment 13
segment_num = 13;
num_pts(segment_num) = 2e2;
delta_vals(segment_num) = -0.1;
F_x_vals(segment_num) = -2000;
delta_vals(segment_num) = -0.03;
F_x_vals(segment_num) = 0;
% Segment 14
segment_num = 14;
num_pts(segment_num) = 3e2;
delta_vals(segment_num) = 0.175;
F_x_vals(segment_num) = -2000;
num_pts(segment_num) = 4e2;
delta_vals(segment_num) = -0.0125;
F_x_vals(segment_num) = 0;
% Segment 15
segment_num = 15;
num_pts(segment_num) = 4.75e2;
delta_vals(segment_num) = 0.0025;
F_x_vals(segment_num) = 1000;
num_pts(segment_num) = 4e2;
delta_vals(segment_num) = 0.05;
F_x_vals(segment_num) = 0;
% Segment 16
segment_num = 16;
num_pts(segment_num) = 4.5e2;
delta_vals(segment_num) = 0.05;
num_pts(segment_num) = 6e2;
delta_vals(segment_num) = 0.015;
F_x_vals(segment_num) = 0;
% Segment 17
segment_num = 17;
num_pts(segment_num) = 5e2;
delta_vals(segment_num) = 0.0;
F_x_vals(segment_num) = 500;
num_pts(segment_num) = 7e2;
delta_vals(segment_num) = -0.045;
F_x_vals(segment_num) = 0;
% Segment 18
segment_num = 18;
num_pts(segment_num) = 8e2;
delta_vals(segment_num) = -0.05;
F_x_vals(segment_num) = -500;
num_pts(segment_num) = 2e2;
delta_vals(segment_num) = -0.07;
F_x_vals(segment_num) = -1000;
% Segment 19
segment_num = 19;
num_pts(segment_num) = 5.8e2;
delta_vals(segment_num) = 0.065;
num_pts(segment_num) = 5.5e2;
delta_vals(segment_num) = 0.071;
F_x_vals(segment_num) = 0;
% Segment 20
segment_num = 20;
num_pts(segment_num) = 7.5e2;
num_pts(segment_num) = 1e2;
delta_vals(segment_num) = 0;
F_x_vals(segment_num) = 2000;
F_x_vals(segment_num) = 2500;
% Segment 21
segment_num = 21;
num_pts(segment_num) = 2.25e2;
delta_vals(segment_num) = 0.5;
F_x_vals(segment_num) = -4400;
num_pts(segment_num) = 5e2;
delta_vals(segment_num) = 0.001;
F_x_vals(segment_num) = 2500;
% Segment 22
segment_num = 22;
num_pts(segment_num) = 9.5e2;
delta_vals(segment_num) = 0.0;
F_x_vals(segment_num) = 5000;
num_pts(segment_num) = 2e2;
delta_vals(segment_num) = 0.02;
F_x_vals(segment_num) = -800;
% Segment 23
segment_num = 23;
num_pts(segment_num) = 2e2;
delta_vals(segment_num) = 0.0165;
F_x_vals(segment_num) = 0;
% Segment 24
segment_num = 24;
num_pts(segment_num) = 1e2;
delta_vals(segment_num) = -0.005;
F_x_vals(segment_num) = 0;
% Segment 25
segment_num = 25;
num_pts(segment_num) = 8e2;
delta_vals(segment_num) = 0;
F_x_vals(segment_num) = 2500;
%% Load Inputs from File
load('ROB535_ControlProject_part1_Team3.mat');
@@ -195,19 +213,19 @@ for i = 1:length(num_pts)
plot(Y(start_idx:end_idx,1), Y(start_idx:end_idx,3), '-');
end
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(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');
plot(Y_submission(:,1), Y_submission(:,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.-');
%% Functions
function [start_idx, end_idx] = get_indices(segment_num, num_pts)