mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-23 19:02:44 +00:00
6cde3df187
- Rename `sravan_part_1.m` to `part1_generate_trajectory.m` - Save inputs to variable called `ROB535_ControlProject_part1_input` - Save variable to `ROB535_ControlProject_part1_Team3.mat`
206 lines
4.2 KiB
Matlab
206 lines
4.2 KiB
Matlab
%% 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;
|
|
|
|
%% 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)
|
|
|
|
% Figures
|
|
close all;
|
|
|
|
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');
|
|
|
|
%% 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 |