%% Cleaning
clc;
clear all
close all

%% Load data
Tvz = 0.00005; 
struct = load ('speed_data.mat');
data = struct.data;
time = 0:Tvz:(length(data)-1)*Tvz;

%% Identification
Ks = mean(data(end-100:end));
T_pom = find(data>Ks*0.632);
T = Tvz * T_pom(1);
p = tf('s');
Fs = Ks / (T*p+1);
step_aprox = step (Fs,time);

figure(1)
plot(time',data)
hold on
plot(time',step_aprox,'Color',[0 0.6 0]);
hold on
line([time(1) time(end)],[Ks Ks],'Color',[0.7 0 0],'LineStyle','--')
line([time(1) 0.125],[data(T_pom(1)) data(T_pom(1))],'Color',[0.7 0 0],'LineStyle','--')
line([T T],[0 2],'Color',[0.7 0 0],'LineStyle','--')

xlabel('Čas [s]')
ylabel('\omega [ot/min]')
legend('Naměřené hodnoty','Aproximace')
grid on
axis([0 0.5 0 3.5])

%% Regulator parameters
Kp = 1.5;
Ti = 0.08;
Fr = Kp*(1 + 1/(Ti*p));

%% Plots
Fw = Fr*Fs/(1+Fr*Fs);
Fx = Fr/(1+Fr*Fs);
figure(3)
plot(time,step(Fw,time))
hold on
plot(time,step(Fx,time),'Color',[0 0.6 0])
grid on
axis([0 0.5 0 1.5])
set(gca,'xtick',0:0.05:0.5)
set(gca,'ytick',0:0.2:1.5)
xlabel('Čas [s]')
ylabel('Amplituda')
legend('Přenos řízení','Přenos akční veličiny')