%% Cleaning
clc;
clear all
close all

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

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

figure(1)
plot(time',data)
hold on
plot(time',step_aprox,'Color',[0 0.6 0]);
hold on
line([time(1) time(300)],[Ks Ks],'Color',[0.7 0 0],'LineStyle','--')
line([time(1) 0.003],[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('I_{d}')
legend('Naměřené hodnoty','Aproximace')
grid on
axis([0 time(300) 0 3.5])


%% Regulator parameters
Fs = Fsaprox * pade(exp(-1.5*Tvz*p),3);

Kp = 1.5;
Ti = 0.0015;
Fr = Kp*(1 + 1/(Ti*p));

%% Plots
Fw = Fr*Fs/(1+Fr*Fs);
Fx = Fr/(1+Fr*Fs);
figure(2)
plot(time,step(Fw,time))
hold on
plot(time,step(Fx,time),'Color',[0 0.6 0])
grid on

set(gca,'xtick',0:0.001:time(300))
set(gca,'ytick',-15:0.25:15)
xlabel('Čas [s]')
ylabel('Amplituda')
legend('Přenos řízení','Přenos akční veličiny')

axis([0 0.009 0 1.75])