Working on Generating Trajectory Manually

- Move provided files out of "SimPkg_F21(student_ver)" folder
- Add `sravan_part_1.m` that generates segments of trajectory
- Currently at 57.34% track completion

sravan_part_1.m

@@ -28,4 +28,166 @@ u_0 = 5; % meters/second
 y_0 = -176; % meters
 v_0 = 0; % meters/second
 psi_0 = 2; % radians
-r_0 = 0; % radians/second
+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;
+[Y,T,U] = simulate_segment(segment_num, [], [], [], num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 2
+segment_num = 2;
+num_pts(segment_num) = 4e2;
+delta_vals(segment_num) = -0.3;
+F_x_vals(segment_num) = -2000;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 3
+segment_num = 3;
+num_pts(segment_num) = 1e2;
+delta_vals(segment_num) = -0.05;
+F_x_vals(segment_num) = 0;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 4
+segment_num = 4;
+num_pts(segment_num) = 7.5e2;
+delta_vals(segment_num) = 0.0;
+F_x_vals(segment_num) = 1000;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 5
+segment_num = 5;
+num_pts(segment_num) = 3e2;
+delta_vals(segment_num) = 0.3;
+F_x_vals(segment_num) = -500;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 6
+segment_num = 6;
+num_pts(segment_num) = 3.5e2;
+delta_vals(segment_num) = -0.03;
+F_x_vals(segment_num) = 1000;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 7
+segment_num = 7;
+num_pts(segment_num) = 1e2;
+delta_vals(segment_num) = -0.005;
+F_x_vals(segment_num) = -1000;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 8
+segment_num = 8;
+num_pts(segment_num) = 2e2;
+delta_vals(segment_num) = 0.025;
+F_x_vals(segment_num) = -750;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 9
+segment_num = 9;
+num_pts(segment_num) = 2.5e2;
+delta_vals(segment_num) = 0.5;
+F_x_vals(segment_num) = -500;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 10
+segment_num = 10;
+num_pts(segment_num) = 5e2;
+delta_vals(segment_num) = -0.02;
+F_x_vals(segment_num) = 0;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 11
+segment_num = 11;
+num_pts(segment_num) = 2.5e2;
+delta_vals(segment_num) = -0.05;
+F_x_vals(segment_num) = 500;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 12
+segment_num = 12;
+num_pts(segment_num) = 2e2;
+delta_vals(segment_num) = -0.01;
+F_x_vals(segment_num) = 5000;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 13
+segment_num = 13;
+num_pts(segment_num) = 2e2;
+delta_vals(segment_num) = -0.1;
+F_x_vals(segment_num) = -2000;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 14
+segment_num = 14;
+num_pts(segment_num) = 3e2;
+delta_vals(segment_num) = 0.175;
+F_x_vals(segment_num) = -2000;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 15
+segment_num = 15;
+num_pts(segment_num) = 5e2;
+delta_vals(segment_num) = -0.01;
+F_x_vals(segment_num) = 1000;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+%% Trajectory Synthesis: Segment 16
+segment_num = 16;
+num_pts(segment_num) = 5e2;
+delta_vals(segment_num) = 0.05;
+F_x_vals(segment_num) = 0;
+[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+% Figures
+close all;
+clc;
+
+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');
+
+info = getTrajectoryInfo(Y,U)
+
+%% Functions
+function [Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0)
+    [start_idx, end_idx] = get_indices(segment_num, num_pts);
+    
+    if segment_num ~= 1
+        state_0 = Y(start_idx-1,:);
+    end
+
+    delta = delta_vals(segment_num);
+    F_x = F_x_vals(segment_num);
+    
+    U_segment = [delta * ones(num_pts(segment_num),1), F_x * ones(num_pts(segment_num),1)];
+    [Y_segment, T_segment] = forwardIntegrateControlInput(U_segment, state_0);
+
+    Y(start_idx:end_idx,:) = Y_segment;
+    T(start_idx:end_idx)   = T_segment;
+    U(start_idx:end_idx,:) = U_segment;
+end
+
+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