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( ... optimoptions( ...
'fmincon', ... 'fmincon', ...
'SpecifyConstraintGradient', false, ... 'SpecifyConstraintGradient', false, ...
'SpecifyObjectiveGradient', true ... 'SpecifyObjectiveGradient', false ...
); );
% Constraint Parameters % Constraint Parameters
@@ -115,16 +115,16 @@ classdef MPC_Class
obj.Y_curr = Y_curr; obj.Y_curr = Y_curr;
obj.Y_ref = Y_ref; obj.Y_ref = Y_ref;
obj.U_ref = U_ref; obj.U_ref = U_ref;
obj.ref_idx = get_ref_index(); obj.ref_idx = obj.get_ref_index();
% Calculate decision variable related quantities % Calculate decision variable related quantities
obj.npred = obj.T_p / obj.T_s; obj.npred = obj.T_p / obj.T_s;
obj.npredstates = npred + 1; obj.npredstates = obj.npred + 1;
obj.npredinputs = npred; obj.npredinputs = obj.npred;
obj.ndecstates = npredstates * nstates; obj.ndecstates = obj.npredstates * obj.nstates;
obj.ndecinputs = npredinputs * ninputs; obj.ndecinputs = obj.npredinputs * obj.ninputs;
obj.ndec = ndecstates + ndecinputs; obj.ndec = obj.ndecstates + obj.ndecinputs;
% Check if `Y_curr` is within prediction horizon of % Check if `Y_curr` is within prediction horizon of
% the end of `Y_ref` % the end of `Y_ref`
@@ -320,10 +320,10 @@ classdef MPC_Class
% Use closest boundary points +/- ntrack_boundary_pts % Use closest boundary points +/- ntrack_boundary_pts
% to construct track polygon % to construct track polygon
bl_idx_start = clamp(bl_idx-obj.ntrack_boundary_pts, 1, size(obj.TestTrack.bl,2)); bl_idx_start = obj.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)); bl_idx_end = obj.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_start = obj.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)); br_idx_end = obj.clamp(br_idx+obj.ntrack_boundary_pts, 1, size(obj.TestTrack.br,2));
boundary_pts = [ ... boundary_pts = [ ...
obj.TestTrack.bl(:,bl_idx_start:1:bl_idx_end), ... obj.TestTrack.bl(:,bl_idx_start:1:bl_idx_end), ...
@@ -431,14 +431,14 @@ classdef MPC_Class
subs( ... subs( ...
A_c_symb, ... 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], ... [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( ... B_c = @(i) double( ...
subs( ... subs( ...
B_c_symb, ... 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], ... [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 end
% Apply input limits (Table 1) % Apply input limits (Table 1)
delta_f = clamp(delta_f, obj.delta_lims(1), obj.delta_lims(2)); delta_f = obj.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)); F_x = obj.clamp(F_x, obj.F_x_lims(1), obj.F_x_lims(2));
end end
end end
@@ -552,5 +552,20 @@ classdef MPC_Class
end end
end 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 end