%% Satellite - Scenario 1a
%
% Assignment 1 in Fuzzy systems
% 
% author:
%   Christos Choutouridis ΑΕΜ 8997
%   cchoutou@ece.auth.gr
%

% 
clear; clc; close all;

s = tf('s');

% Givens
% -------------------------------------------------------
Gp = 10/((s+1)*(s+9));          % given plant
spec.Mp_max   = 0.07;           % 7% overshoot
spec.Tr_max   = 0.6;            % 0.6sec rise time
spec.SteadyStateValue = 1.0;    % step of 1

Ts = 0.01;

% Configuration
tEnd = 5;
Ke  = 1;
Kde=0.29;   % Calculated with scenario1_tune.m
Kdu=0.33;
fis = mkFIS();

Gz = c2d(Gp, Ts, 'zoh');
Gd = ss(Gz);
Ad=Gd.A; Bd=Gd.B; Cd=Gd.C; Dd=Gd.D;


t = 0:Ts:tEnd;
r = ones(size(t));
x = zeros(size(Ad,1),1);
y = 0;

% logs
Y = zeros(size(t)); U = zeros(size(t)); E = zeros(size(t));

clear satFZ_PI;  % Init satFZ_PI
for k = 1:numel(t)
    % calculate error
    e = r(k) - y;

    %evaluate controller
    u = satFZ_PI(fis, e, Ke, Kde, Kdu, Ts);

    % plant update (ZOH)
    x = Ad*x + Bd*u;
    y = Cd*x + Dd*u;

    % keep logs
    Y(k) = y; U(k) = u; E(k) = e;
end
info = stepinfo(Y, t, 1);


f1 = figure( ...
    'Color','w', ...
    'Name',sprintf('Step responce - Kde=%.3g, Kdu=%.3g', Kde, Kdu), ...
    'Position',[200 200 1200 800] ...
);
plot(t, Y);
xlabel('Time (s)');
ylabel('Amplitude');
title(sprintf('Step responce - Kde=%.3g, Kdu=%.3g', Kde, Kdu));
grid on;

infoStep = sprintf(['RiseTime   : %.4g s\n' ...
                    'Settling   : %.4g s\n' ...
                    'Overshoot  : %.2f %%\n' ...
                    'Peak       : %.4g (at %.4g s)\n' ...
                    'SS error   : %.3f'], ...
                   info.RiseTime, info.SettlingTime, info.Overshoot, ...
                   info.Peak, info.PeakTime, E(end));

annotation(f1, 'textbox', [0.7 0.05 0.4 0.25], ...
    'String', infoStep, 'Interpreter','tex', ...
    'FontName','monospaced', 'FontSize',12, ...
    'FitBoxToText','on', 'BackgroundColor','w', 'EdgeColor',[1 1 1]);


% save figure
exportgraphics(f1, sprintf('Step Responce_FZ.png'), 'Resolution', 300);
% exportgraphics(f1, sprintf('Step Responce_FZ.pdf'));