% =========================================================================
%   Praktikum MATLAB/Simulink I
%   Technische Universität Darmstadt
%   IAT, FG rtm (Prof. Dr.-Ing. U. Konigorski)
%
%   Copyright (c) 2017, FG Regelungstechnik und Mechatronik,
%                       Technische Universität Darmstadt
%   All rights reserved.
%
%   Redistribution and use in source and binary forms, with or withou
%   modification, are permitted provided that the following conditions are
%   met:
%
%   1. Redistributions of source code must retain the above copyright
%      notice, this list of conditions and the following disclaimer.
%
%   2. Redistributions in binary form must reproduce the above copyright
%      notice, this list of conditions and the following disclaimer in the
%      documentation and/or other materials provided with the distribution.
%
%
%   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
%   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
%   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
%   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
%   HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
%   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
%   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
%   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
%   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
%   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
%   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

% =========================================================================

% Versuch 5.3
clc
clear all

% Initialisierung der Parameter
Parameter

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%% Versuchsdurchführung 5.3.1 %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Eingabe der Übertragungsfunktion

% Übertragungsfunktion Ventilators
G1 = tf(cM, [(L*JP) (R*JP + K*L) (K*R + cM^2)]);

% Übertragungsfunktion Pendels
G2 = tf(2 * gamma * w_s * HR, [JW cW (-K * sin(alpha_s))]);

disp('Übertragungsfunktion des Pendelschraubers:')
G = G1 * G2

%% Ablesen der Zeitkonstanten für den Reglerentwurf

T1 = 1 / 3.099e4;   % =3.2268e-5
T2 = 1 / 1.691;     % =0.5914
T3 = 1 / 0.3918;    % =2.5523

%% Reglerübertragungsfunktion

% Auswahl der dominanten Polstellen
G_R = tf([T2*T3 T2+T3 1], [1 0]);
% Ausgabe der Reglerübertragungsfunktion
disp('Reglerübertragungsfunktion')
G_R = zpk(G_R)

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%% Versuchsdurchführung 5.3.3 %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Berechnung der offenen Strecke mit Regler und Zeichnen des Bodediagramms

RG = G_R * G;

[mag, phase, wout] = bode(RG, {0.1, 1e5});
figure();
subplot(2, 1, 1)
semilogx(wout, 10 * log(mag(:,:)), 'linewidth', 2);
grid on;
xlim([0.1 1e5]);
ylim([-250 50])
ylabel('Magnitude (dB)');
set(gca(), 'XTickLabel', {''});
title('Bode Diagram');
subplot(2, 1, 2)
semilogx(wout, phase(:,:), 'linewidth', 2);
grid on;
xlim([0.1 1e5]);
ylim([-270 -135]);
ylabel('Phase (deg)');
xlabel('Frequency (rad/sec)')



%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%% Versuchsdurchführung 5.3.4 %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Verschiebung der Nullstellen für die Phasenanhebung
G_R = zpk([-0.1691 -0.3918], 0, 1.5094);

% Übertragungsfunktion der offenen Strecke mit neuem Reglern
RG = G_R * G;

% Zeichnen des Bodediagramms
figure('name', 'Versuchsdurchführung 5.3.4-Bodediagramm')
bode(RG, {1e-4, 1e5})
grid on



%% Ablesen der Betragssenkung und Berechnung des Verstärkungsfaktors
V = 10^(-9.72/20); % =0.3266

% Eingabe der neuen Übertragungsfunktion
G_R = V * G_R;
KRG = G_R * G;

% Zeichnen der Sprungantwort
figure('name', 'Versuchsdurchführung 5.3.4-Sprungantwort')
step(feedback(KRG, 1))


[num_R, den_R] = tfdata(G_R, 'v');

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%% Versuchsdurchführung 5.3.6 %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Ergänzung um einen weiteren Pol um Kausalität zu gewährleisten
T = 0.01;
final = 0.2;

s = tf('s');
G_R = 1 / (T*s + 1) * G_R;

[num_R, den_R] = tfdata(G_R, 'v');

% Endwert der Sprungantwort
sim('Pendelschrauber', 100);
y = ones(1, length(alpha_rad.time)) * ...
        alpha_rad.signals.values(length(alpha_rad.time), 1) * 180/pi;

figure('name', 'Versuchsdurchführung 5.3.6-Simulationsergebnis');
set(0, 'defaultTextInterpreter', 'latex'); 
plot(alpha_rad.time, alpha_rad.signals.values * 180/pi, 'linewidth', 2);
hold on;
plot(alpha_rad.time, y, '--k');
xlabel('Time (sec)', 'interpreter', 'latex');
ylabel('$$\alpha\ (^\circ)$$');
ylim([0 18]);
xlim([0 100]);
grid on;
title('Step Response');

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%% Versuchsdurchführung 5.3.8 %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% PIDT1-Regler als Parallelschaltung
% Berechnung der Parameter KR, KI und KD

KI = T * num_R(3);
KP = (num_R(2) - KI) * T;
KD = (num_R(1) - KP) * T;


% Endwert der Sprungantwort
final=0.2;

sim('PendelschrauberParallelschaltung', 120);
t = PAnteil.time;
UPAnteil = PAnteil.signals.values;
UDAnteil = DAnteil.signals.values;
UIAnteil = IAnteil.signals.values;

figure('name', 'Versuchsdurchführung 5.3.8-Simulationsergebnis')
subplot(3, 1, 1)
plot(t, UPAnteil, 'linewidth', 2);
ylim([-9e-2 9e-2]);
xlim([0 120]); 
title('P-Anteil')
xlabel('Zeit (sec)')
ylabel('Spannung (V)')
grid on

subplot(3, 1, 2)
plot(t, UDAnteil, 'linewidth', 2);
ylim([-1 1]);
xlim([0 120]); 
title('D-Anteil')
xlabel('Zeit (sec)')
ylabel('Spannung (V)')
grid on

subplot(3, 1, 3)
plot(t, UIAnteil, 'linewidth', 2)
ylim([-4e-2 4e-2]);
xlim([0 120]); 
title('I-Anteil')
xlabel('Zeit (sec)')
ylabel('Spannung (V)')
grid on


%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%% Versuchsdurchführung 5.3.9 %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% PIDT1-Regler am nichtlinearen Modell
% Endwert der Sprungantwort
final=0.2;

sim('PendelschrauberNichtlin', 300);
y = ones(1, length(alpha_rad_nichtl.time)) ...
        * alpha_rad.signals.values(length(alpha_rad.time), 1) * 180/pi;

figure('name', 'Versuchsdurchführung 5.3.9-Simulationsergebnis am nichtlinearen Modell')
plot(alpha_rad_nichtl.time, alpha_rad_nichtl.signals.values * 180/pi, 'linewidth', 2);
hold on;
plot(alpha_rad_nichtl.time, y, '--k');
xlabel('Time (sec)');
ylabel('$$\alpha\ (^\circ)$$');
ylim([-120 60]);
xlim([0 300]);
set(gca(), 'YTick', (-120:30:30));
set(gca(), 'YTickLabel', {'-120', '-90', '-60', '-30', '0', '30'});
grid on;
title('Step Response');

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%% Versuchsdurchführung 5.3.12 %%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% PIDT1-Regler als Parallelschaltung am nichtlinearen Modell

% Endwert der Sprungantwort
final = 0.2;

sim('PendelschrauberParallelschaltungNichtlin', 300);
t = PAnteilNichtlin.time;
UPAnteilNichtlin = PAnteilNichtlin.signals.values;
UDAnteilNichtlin = DAnteilNichtlin.signals.values;
UIAnteilNichtlin = IAnteilNichtlin.signals.values;

figure('name', 'Versuchsdurchführung 5.3.12-Simulationsergebnis nichtlineares Modell')
subplot(3, 1, 1)
plot(t, UPAnteilNichtlin, 'linewidth', 2);
ylim([-1 1]);
xlim([0 300]); 
title('P-Anteil')
xlabel('Zeit (sec)')
ylabel('Spannung (V)')
grid on

subplot(3, 1, 2)
plot(t, UDAnteilNichtlin, 'linewidth', 2);
ylim([-1 1]);
xlim([0 300]); 
title('D-Anteil')
xlabel('Zeit (sec)')
ylabel('Spannung (V)')
grid on

subplot(3, 1, 3)
plot(t, UIAnteilNichtlin, 'linewidth', 2)
ylim([0 4]);
xlim([0 300]); 
title('I-Anteil')
xlabel('Zeit (sec)')
ylabel('Spannung (V)')
grid on

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%% Versuchsdurchführung 5.3.13 %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% PIDT1-Regler am nichtlinearen Modell
% Endwert der Sprungantwort

final = 0.1;

sim('PendelschrauberNichtlin', 300);
y = ones(1,length(alpha_rad_nichtl.time)) * final * 180/pi;

figure('name', 'Versuchsdurchführung 5.3.9-Simulationsergebnis am nichtlinearen Modell')
plot(alpha_rad_nichtl.time, alpha_rad_nichtl.signals.values * 180/pi, 'linewidth', 2);
hold on;
plot(alpha_rad_nichtl.time, y, '--k');
xlabel('Time (sec)');
ylabel('$$\alpha\ (^\circ)$$');
ylim([-120 25]);
xlim([0 300]);
set(gca(), 'YTick', (-120:30:30));
set(gca(), 'YTickLabel', {'-120', '-90', '-60', '-30', '0', '30'});
grid on;
title('Step Response');
    

