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

Fix Missing References & Add clamp Function

Browse Source 
- Change `SpecifyObjectiveGradient` to `false`
- Fix missing references to `obj`
- Add missing `clamp` function
This commit is contained in:
Sravan Balaji
2021-12-12 14:50:11 -05:00
parent 2d3304ace1
commit 6361cfd40c
1 changed files with 30 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
Experimentation/MPC_Class.m
View File

@@ -95,7 +95,7 @@ classdef MPC_Class
optimoptions( ...
'fmincon', ...
'SpecifyConstraintGradient', false, ...
'SpecifyObjectiveGradient', true ...
'SpecifyObjectiveGradient', false ...
);
% Constraint Parameters
@@ -115,16 +115,16 @@ classdef MPC_Class
obj.Y_curr = Y_curr;
obj.Y_ref = Y_ref;
obj.U_ref = U_ref;
obj.ref_idx = get_ref_index();
obj.ref_idx = obj.get_ref_index();
% Calculate decision variable related quantities
obj.npred = obj.T_p / obj.T_s;
obj.npredstates = npred + 1;
obj.npredinputs = npred;
obj.npredstates = obj.npred + 1;
obj.npredinputs = obj.npred;
obj.ndecstates = npredstates * nstates;
obj.ndecinputs = npredinputs * ninputs;
obj.ndec = ndecstates + ndecinputs;
obj.ndecstates = obj.npredstates * obj.nstates;
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`
@@ -320,10 +320,10 @@ classdef MPC_Class
% Use closest boundary points +/- ntrack_boundary_pts
% to construct track polygon
bl_idx_start = clamp(bl_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2));
bl_idx_end = clamp(bl_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2));
br_idx_start = clamp(br_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2));
br_idx_end = clamp(br_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2));
bl_idx_start = obj.clamp(bl_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2));
bl_idx_end = obj.clamp(bl_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2));
br_idx_start = obj.clamp(br_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2));
br_idx_end = obj.clamp(br_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2));
boundary_pts = [ ...
obj.TestTrack.bl(:,bl_idx_start:1:bl_idx_end), ...
@@ -431,14 +431,14 @@ classdef MPC_Class
subs( ...
A_c_symb, ...
[x_var, u_var, y_var, v_var, psi_var, r_var, delta_f_var, F_x_var, F_yf_var, F_yr_var], ...
bike_model_helper(obj.Y_ref(obj.ref_idx+i,:), obj.U_ref(obj.ref_idx+i,:)) ...
obj.bike_model_helper(obj.Y_ref(obj.ref_idx+i,:), obj.U_ref(obj.ref_idx+i,:)) ...
) ...
);
B_c = @(i) double( ...
subs( ...
B_c_symb, ...
[x_var, u_var, y_var, v_var, psi_var, r_var, delta_f_var, F_x_var, F_yf_var, F_yr_var], ...
bike_model_helper(obj.Y_ref(obj.ref_idx+i,:), obj.U_ref(obj.ref_idx+i,:)) ...
obj.bike_model_helper(obj.Y_ref(obj.ref_idx+i,:), obj.U_ref(obj.ref_idx+i,:)) ...
) ...
);
@@ -491,8 +491,8 @@ classdef MPC_Class
end
% Apply input limits (Table 1)
delta_f = clamp(delta_f, obj.delta_lims(1), obj.delta_lims(2));
F_x = clamp(F_x, obj.F_x_lims(1), obj.F_x_lims(2));
delta_f = obj.clamp(delta_f, obj.delta_lims(1), obj.delta_lims(2));
F_x = obj.clamp(F_x, obj.F_x_lims(1), obj.F_x_lims(2));
end
end
@@ -552,5 +552,20 @@ classdef MPC_Class
end
end
end
%% Static Helper Functions
methods (Static)
function clamped_val = clamp(val, min, max)
%clamp Limits a value to range: min <= val <= max
clamped_val = val;
if clamped_val < min
clamped_val = min;
elseif clamped_val > max
clamped_val = max;
end
end
end
end