% Sestaveni pohonu osy s jednostrannym nahonem dlouheho sroubu
clear;
clc;

%% nacitani dat
% Poloha merena od zacatku KS, ne od 0 polohy

pol_ind=7;
switch pol_ind
    case 1
        load('Loze_0_SS.mat');
        poloha=0.35;
        [Drive_modelZ,Bearing_modelZ,LockNut_modelZ]=Pohon_Z(poloha,1);
        
    case 2
        load('Loze_600_SS.mat');
        poloha=0.95;
        [Drive_modelZ,Bearing_modelZ,LockNut_modelZ]=Pohon_Z(poloha,1);
        
    case 3
        load('Loze_1500_SS.mat');
        poloha=1.85;
        [Drive_modelZ,Bearing_modelZ,LockNut_modelZ]=Pohon_Z(poloha,1);
        %[Drive_modelZ,Bearing_modelZ,LockNut_modelZ]=Pohon_Z(poloha,2);
        
    case 4
        load('Loze_3000_SS.mat');
        poloha=3.35;
        [Drive_modelZ,Bearing_modelZ,LockNut_modelZ]=Pohon_Z(poloha,2);
        
    case 5
        load('Loze_4500_SS.mat');
        poloha=4.85;
        [Drive_modelZ,Bearing_modelZ,LockNut_modelZ]=Pohon_Z(poloha,3);
        %[Drive_modelZ,Bearing_modelZ,LockNut_modelZ]=Pohon_Z(poloha,2);
        
    case 6
        load('Loze_5400_SS.mat');
        poloha=5.75;
        [Drive_modelZ,Bearing_modelZ,LockNut_modelZ]=Pohon_Z(poloha,3);
        
    case 7
        load('Loze_6000_SS.mat');
        poloha=6.35;
        [Drive_modelZ,Bearing_modelZ,LockNut_modelZ]=Pohon_Z(poloha,3);
end;

MKPin=ABCDlti_skup.InputName;
MKPon=ABCDlti_skup.OutputName;

% step(ABCDlti_skup({'Stul_z'},{'Stul_Fz'}))

Drive_modelZ.InputName={'FfimZ';'FfisZ';'Fx1Z';'FxsZ'};
Drive_modelZ.OutputName={'fimZ';'fisZ';'x1Z';'xsZ';'fim_tZ';'fis_tZ';'x1_tZ';'xs_tZ'};

Bearing_modelZ.InputName={'Konzola_1_z';'x1Z';'Konzola_1_x_t';'x1_tZ'};
Bearing_modelZ.OutputName={'Konzola_1_Fz';'Fx1Z'};

LockNut_modelZ.InputName={'Matice_z';'xsZ';'fisZ';'Matice_z_t';'xs_tZ';'fis_tZ'};
LockNut_modelZ.OutputName={'Matice_Fz';'FxsZ';'FfisZ'};

odmZ=ss([-1 1]);
odmZ.InputName={'Pravitko_z';'Jezdec_z'};
odmZ.OutputName={'Odm_Z'};

mech_in=[MKPin;{'FfimZ';'FfisZ';'Fx1Z';'FxsZ'}];
mech_on=[MKPon;{'fimZ';'fisZ';'x1Z';'xsZ';'fim_tZ';'fis_tZ';'x1_tZ';'xs_tZ'};{'Odm_Z'}];

propojeny_model=connect(ABCDlti_skup,Drive_modelZ,Bearing_modelZ,LockNut_modelZ,odmZ,mech_in,mech_on);

%%
%Kontroly spravneho propojeni modelu (spravna pohyblivost):
%step(propojeny_model({'fimZ';'fim_tZ';'Odm_Z';'Stul_z'},{'FfimZ'}))

% Skok sily - na zatez (stul) 
% step(propojeny_model({'fimZ';'fim_tZ';'Odm_Z';'Stul_z'},{'Stul_Fz'}))

% hledani nejpoddajnejsiho mista
% figure; bode(propojeny_model({'fim_tZ';'Odm_Z'},{'FfimZ'}))
% figure; bode(propojeny_model({'fim_tZ'},{'FfimZ'}))

%% rychlostni regulace
[proudova_regulace_Z,filtry_proudu_Z,rychlostni_regulace_Z,polohova_regulace_Z]=regulaceZ;
proudova_regulace_Z.InputName={'Izad_filt_Z'};
proudova_regulace_Z.OutputName={'FfimZ'};

filtry_proudu_Z.InputName={'Izad_Z'};
filtry_proudu_Z.OutputName={'Izad_filt_Z'};

rychlostni_regulace_Z.InputName={'om_zad_Z','fim_tZ'};
rychlostni_regulace_Z.OutputName={'Izad_Z'};

rych_in=[mech_in;{'om_zad_Z'}];
rych_on=[mech_on;{'FfimZ';'Izad_filt_Z';'Izad_Z'}];

rych_regulace=connect(propojeny_model,...
    proudova_regulace_Z,filtry_proudu_Z,rychlostni_regulace_Z,...
    rych_in,rych_on);

%Testovani rychlostni regulace
%  figure;step(rych_regulace({'fim_tZ'},{'om_zad_Z'}));figure;bode(rych_regulace({'fim_tZ'},{'om_zad_Z'}));grid on; zoom on;

%% polohova regulace
polohova_regulace_Z.InputName={'Z_zad','Odm_Z','v_zadFFWD_Z'};
polohova_regulace_Z.OutputName={'Delta_Z','om_zad_Z'};

pol_in=[rych_in;{'Z_zad';'v_zadFFWD_Z'}];
pol_on=[rych_on;{'Delta_Z';'om_zad_Z'}];

pol_regulace=connect(rych_regulace,polohova_regulace_Z,...
    pol_in,pol_on);

%Testovani polohove regulace - odezva na rampu polohy
StartTime=1;
StopTime=3;
InitVal=0;
FinVal=0.2;
t=(0:0.0001:5)';
hod=ones(size(t))*InitVal;
dt=StopTime-StartTime;
ds=FinVal-InitVal;
hod=hod+(abs(t-StartTime)-abs(t-StopTime)+dt)/(2*dt)*ds;

y=lsim(pol_regulace({'Delta_Z';'Odm_Z'},{'Z_zad'}),hod,t);
% figure;plot(t,hod,'k',t,y(:,2),'b')
% figure;plot(t,y(:,1))

rych_reg_jedno=rych_regulace({'fim_tZ'},{'om_zad_Z'});
odezva_rampa_jedno=[t hod y];