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

BIN
Deliverables/ROB535_ControlProject_part1_Team3.mat
View File

Binary file not shown.

144
Experimentation/part1_generate_trajectory.m
View File

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