Re-Organize Files Again

- Deliverables folder holds final files for submission
- Experimentation holds scripts and things related to our development of
  a final solution
- Simulation holds the provided files
- Added template for part 2 submission with comments

Experimentation/part1_generate_trajectory.m

@@ -0,0 +1,221 @@
+%% ROB 535 Team 3 Control Project
+close all;
+clear;
+clc;
+
+%% Model Parameters
+% Vehicle Parameters
+delta_lim = [-0.5, 0.5];
+F_x_lim = [-5000, 5000];
+m = 1400;
+N_w = 2;
+f = 0.01;
+I_z = 2667;
+a = 1.35;
+b = 1.45;
+B_y = 0.27;
+C_y = 1.2;
+D_y = 0.7;
+E_y = -1.6;
+S_hy = 0;
+S_vy = 0;
+g = 9.806;
+% Note: The Pacejka parameters are for the slip angle in degrees
+
+% Initial Conditions
+x_0 = 287; % meters
+u_0 = 5; % meters/second
+y_0 = -176; % meters
+v_0 = 0; % meters/second
+psi_0 = 2; % radians
+r_0 = 0; % radians/second
+state_0 = [x_0, u_0, y_0, v_0, psi_0, r_0];
+
+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;
+
+% Segment 2
+segment_num = 2;
+num_pts(segment_num) = 4e2;
+delta_vals(segment_num) = -0.3;
+F_x_vals(segment_num) = -2000;
+
+% Segment 3
+segment_num = 3;
+num_pts(segment_num) = 1e2;
+delta_vals(segment_num) = -0.05;
+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;
+
+% Segment 5
+segment_num = 5;
+num_pts(segment_num) = 3e2;
+delta_vals(segment_num) = 0.3;
+F_x_vals(segment_num) = -500;
+
+% Segment 6
+segment_num = 6;
+num_pts(segment_num) = 3.5e2;
+delta_vals(segment_num) = -0.03;
+F_x_vals(segment_num) = 1000;
+
+% Segment 7
+segment_num = 7;
+num_pts(segment_num) = 1e2;
+delta_vals(segment_num) = -0.005;
+F_x_vals(segment_num) = -1000;
+
+% Segment 8
+segment_num = 8;
+num_pts(segment_num) = 2e2;
+delta_vals(segment_num) = 0.0275;
+F_x_vals(segment_num) = -750;
+
+% Segment 9
+segment_num = 9;
+num_pts(segment_num) = 2.4e2;
+delta_vals(segment_num) = 0.5;
+F_x_vals(segment_num) = -500;
+
+% Segment 10
+segment_num = 10;
+num_pts(segment_num) = 5e2;
+delta_vals(segment_num) = -0.02;
+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;
+
+% Segment 12
+segment_num = 12;
+num_pts(segment_num) = 2e2;
+delta_vals(segment_num) = -0.01;
+F_x_vals(segment_num) = 5000;
+
+% Segment 13
+segment_num = 13;
+num_pts(segment_num) = 2e2;
+delta_vals(segment_num) = -0.1;
+F_x_vals(segment_num) = -2000;
+
+% Segment 14
+segment_num = 14;
+num_pts(segment_num) = 3e2;
+delta_vals(segment_num) = 0.175;
+F_x_vals(segment_num) = -2000;
+
+% Segment 15
+segment_num = 15;
+num_pts(segment_num) = 4.75e2;
+delta_vals(segment_num) = 0.0025;
+F_x_vals(segment_num) = 1000;
+
+% Segment 16
+segment_num = 16;
+num_pts(segment_num) = 4.5e2;
+delta_vals(segment_num) = 0.05;
+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;
+
+% Segment 18
+segment_num = 18;
+num_pts(segment_num) = 8e2;
+delta_vals(segment_num) = -0.05;
+F_x_vals(segment_num) = -500;
+
+% Segment 19
+segment_num = 19;
+num_pts(segment_num) = 5.8e2;
+delta_vals(segment_num) = 0.065;
+F_x_vals(segment_num) = 0;
+
+% Segment 20
+segment_num = 20;
+num_pts(segment_num) = 7.5e2;
+delta_vals(segment_num) = 0;
+F_x_vals(segment_num) = 2000;
+
+% Segment 21
+segment_num = 21;
+num_pts(segment_num) = 2.25e2;
+delta_vals(segment_num) = 0.5;
+F_x_vals(segment_num) = -4400;
+
+% Segment 22
+segment_num = 22;
+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);
+
+    delta = delta_vals(i);
+    F_x = F_x_vals(i);
+    U(start_idx:end_idx,:) = [delta * ones(num_pts(i),1), F_x * ones(num_pts(i),1)];
+end
+
+[Y, T] = forwardIntegrateControlInput(U, state_0);
+info = getTrajectoryInfo(Y,U)
+
+[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;
+
+for i = 1:length(num_pts)
+    [start_idx, end_idx] = get_indices(i, 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 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
+        start_idx = 1;
+        end_idx = num_pts(segment_num);
+    else
+        start_idx = sum(num_pts(1:segment_num-1)) + 1;
+        end_idx = sum(num_pts(1:segment_num));
+    end
+end