
%% Main script for generating trade-off curves
% Written by Chriss Grimholt in 2015 to calculate an optimal PI controller for input disturbances.

% Modified by Iosif Pappas in Spring 2016 to calculate the optimal ideal PID controller for input and output disturbances.

% Define the Laplace variable
s = tf('s');

%Set the process model, where k is the process gain, y the time delay and t
%the process time constant.

G = k*exp(-y*s)/(ts+1)

% PID controller, p = [kp + ki + kd]
K = @(p) (p(1) + p(2)/s + p(3)*s);

% The derivative of the ideal PID controller
dK = @(p) [tf(1); 1/s; s];

% Specify the robustness constraint
problem.ms_eqcon =  1.59;
problem.mt_eqcon = problem.ms_eqcon;

% Specify the initial guess
problem.x0 = [kc ki kd];

% Parameters for the step simulation and the gridding of the frequncy domain
problem.time_gridding = linspace(0,20, 1e4);
problem.freqency_gridding = logspace(-10,10,1e4);

problem.opt = optimset('Algorithm','active-set','GradConstr','on','GradObj','on','DerivativeCheck','off','Display','off');



%% Solving the problem
%  ===================

current_date = clock;
fprintf('\n')
fprintf('Date: %i/%i-%i, Time: %i:%i',current_date(3),current_date(2),current_date(1),current_date(4:5))
fprintf('\n')
fprintf('Finding Optimal Controller\n')
fprintf('***************************\n')
fprintf('\n')
fprintf('cost  \t Ms \t Mt \t eFlag \t iter \t time \t\topt_param   \n')
fprintf('----------------------------------------------------------------------------------\n')
%%  
% Create the transfer functions
% The Gang of Four
S = @(p) 1/(1+G*K(p));
GS = @(p) G*S(p);
KS = @(p) K(p)*S(p);

% The gradient of the error
de_dy = @(p) -GS(p)*S(p)*dK(p);
de_di = @(p) -GS(p)*GS(p)*dK(p);

% Adding transfer functions to problem
problem.G  = G;
problem.K  = K;
problem.KS = KS;
problem.dK = dK;
problem.S  = S;
problem.GS = GS;
problem.de_dy = de_dy;
problem.de_di = de_di;
problem.weightu=1.582;
problem.weighty=0.169;
% take the time
tic

% optimize and find the optimal solution (sending the struct problem to
% function opt_problem.

res = opt_problem(problem);
% z(j,1) = res.cost;
% find how long it took to solve the problem
res.calc_time = toc;

% find how many iteration it required
res.iter = res.output.iterations;


% print the current results
fprintf('%0.3f \t %.3f \t %.3f \t %i \t\t %i \t\t %.3fs\t\t', res.cost, res.ms, res.mt, res.exitflag, res.iter, res.calc_time)
for i = 1:length(res.tuning(:))
    fprintf('%.3f  ', res.tuning(i))
end
fprintf('\n')

% end
