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
T_s = 0.01; % Step Size [s]
T_p = 0.5; % Prediction Horizon [s]
T_s = 0.01; % Step Size [s]
T_p = 0.5; % Prediction Horizon [s]
sim_stop_idx = 2e3; % Index in reference trajectory to stop sim
% Decision Variables
nstates = 6; % Number of states per prediction
@@ -126,10 +127,9 @@ classdef MPC_Class
obj.ndecinputs = obj.npredinputs * obj.ninputs;
obj.ndec = obj.ndecstates + obj.ndecinputs;
% Check if `Y_curr` is within prediction horizon of
% the end of `Y_ref`
% Stop simulation when `sim_stop_idx` is reached
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;
end
end

16
Experimentation/part2_test_controller.m
View File

@@ -4,7 +4,7 @@ clear;
clc;
%% Call simulation functions
[Y,U,t_total,t_update] = forwardIntegrate();
[Y,U,t_total,t_update,Xobs] = forwardIntegrate();
info = getTrajectoryInfo(Y,U)
%% Figures
@@ -17,9 +17,15 @@ grid on;
load('TestTrack.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.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
%
% This script returns the vehicle trajectory with control input being