%% Satellite - Scenario 2
%
% 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
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;


% Control inputs
tEnd = 16;
t = 0:Ts:tEnd;

% 2a
r_step = zeros(size(t));
r_step(t < 4)                 = 60;
r_step(t >= 4 & t < 8)        = 20;
r_step(t >= 8)                = 40;

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_step(k)/60 - 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

f1 = figure( ...
    'Color','w', ...
    'Name',sprintf('MultiStep responce'), ...
    'Position',[200 200 1200 800] ...
);
plot(t, r_step, 'k--', 'LineWidth',1.2); hold on;
plot(t, Y*60, 'b', 'LineWidth',1.2);
grid on; xlabel('Time (s)'); ylabel('Amplitude');
title('Scenario 2A: multi-step (60->20->40)');
legend('r','y','Location','best');

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


% 2b
r_trap = zeros(size(t));
ix = (t <= 5);
r_trap(ix) = 60/5 * t(ix);
ix = (t > 5 & t <= 8);
r_trap(ix) = 60;
ix = (t > 8 & t <= 16);
r_trap(ix) = 60 * (1 - (t(ix) - 8)/(16 - 8));

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_trap(k)/60 - 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);


f2 = figure( ...
    'Color','w', ...
    'Name',sprintf('Trapezoid responce'), ...
    'Position',[200 200 1200 800] ...
);
plot(t, r_trap, 'k--', 'LineWidth',1.2); hold on;
plot(t, Y*60, 'b', 'LineWidth',1.2);
grid on; xlabel('Time (s)'); ylabel('Amplitude');
title('Scenario 2B: trapezoid (0->60, hold, 60->0)');
legend('r','y','Location','best');

% save figure
exportgraphics(f2, sprintf('Trapezoid_Responce_FZ.png'), 'Resolution', 300);
% exportgraphics(f2, sprintf('Trapezoid_Responce_FZ.pdf'));