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

Simulation Stop Index & Obstacle Plotting

Browse Source 
- Add parameter to stop simulation when specified
  index in reference trajectory is reached
- Modify `forwardIntegrate` to return `Xobs` for plotting
- Plot obstacles in part2 testing figure
- Change some line plotting styles for clarity
This commit is contained in:
Sravan Balaji
2021-12-12 16:29:20 -05:00
parent 5534819d90
commit 6f6db84c23
3 changed files with 17 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
Deliverables/MPC_Class.m
View File

@@ -54,8 +54,9 @@ classdef MPC_Class
]; ];
% Simulation Parameters % Simulation Parameters
T_s = 0.01; % Step Size [s] T_s = 0.01; % Step Size [s]
T_p = 0.5; % Prediction Horizon [s] T_p = 0.5; % Prediction Horizon [s]
sim_stop_idx = 2e3; % Index in reference trajectory to stop sim
% Decision Variables % Decision Variables
nstates = 6; % Number of states per prediction nstates = 6; % Number of states per prediction
@@ -126,10 +127,9 @@ classdef MPC_Class
obj.ndecinputs = obj.npredinputs * obj.ninputs; obj.ndecinputs = obj.npredinputs * obj.ninputs;
obj.ndec = obj.ndecstates + obj.ndecinputs; obj.ndec = obj.ndecstates + obj.ndecinputs;
% Check if `Y_curr` is within prediction horizon of % Stop simulation when `sim_stop_idx` is reached
% the end of `Y_ref`
obj.FLAG_terminate = 0; obj.FLAG_terminate = 0;
if size(obj.Y_ref, 1) - obj.ref_idx < obj.npred if obj.ref_idx > obj.sim_stop_idx
obj.FLAG_terminate = 1; obj.FLAG_terminate = 1;
end end
end end

16
Experimentation/part2_test_controller.m
View File

@@ -4,7 +4,7 @@ clear;
clc; clc;
%% Call simulation functions %% Call simulation functions
[Y,U,t_total,t_update] = forwardIntegrate(); [Y,U,t_total,t_update,Xobs] = forwardIntegrate();
info = getTrajectoryInfo(Y,U) info = getTrajectoryInfo(Y,U)
%% Figures %% Figures
@@ -17,9 +17,15 @@ grid on;
load('TestTrack.mat') load('TestTrack.mat')
load('ROB535_ControlProject_part1_Team3.mat') load('ROB535_ControlProject_part1_Team3.mat')
plot(Y(:,1), Y(:,3), 'b-');
plot(ROB535_ControlProject_part1_state(:,1), ROB535_ControlProject_part1_state(:,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.-');
for i = 1:length(Xobs)
xvals = Xobs{i}(:,1);
yvals = Xobs{i}(:,2);
fill(xvals, yvals, 'm');
end
plot(ROB535_ControlProject_part1_state(:,1), ROB535_ControlProject_part1_state(:,3), 'k--');
plot(Y(:,1), Y(:,3), 'b-');

2
Simulation/forwardIntegrate.m
View File

@@ -1,4 +1,4 @@
function [Y,U,t_total,t_update] = forwardIntegrate() function [Y,U,t_total,t_update,Xobs] = forwardIntegrate()
% [Y,U,t_total,t_update] = forwardIntegrate % [Y,U,t_total,t_update] = forwardIntegrate
% %
% This script returns the vehicle trajectory with control input being % This script returns the vehicle trajectory with control input being