% This file takes the pieces of the transfer function from
% Towards  Collaborative  Drilling  with  a Cobot  Using  Admittance  Controller
% the work of Aydin et al. The values for all their controller 
% variables G(s) C(s) Zeq(s) and H(s) are featured here.
% Input:
%   A - Admittance controller TF
%   Zeq - equivalent Impedance
% Output:
%   TF - Closed or Open loop transfer functions

% Created by Brevin Banks 
% Modified 3/14/2023
function TF = TFAydin(Ys,Zeq,ClosedorOpen)

% Check function input statements
if nargin==2
    ClosedorOpen='closed';
    warning('No loop type selcted. Returned TF will be closed loop by default');
end
if nargin==1
    ClosedorOpen='closed';
    warning('No impedance condition selected. Returned TF will be open-loop with no Impedance');
    warning("the inner closed loop on the controller still present");
    
end
if nargin==0
    ClosedorOpen='closed';
    warning("No admittance controller selected. Returned TF won't have admittance or impedance applied");
    warning("Input type becomes velocity. Output has closed inner loop on the controller");
end


% Aydin et al paper G(s) values from system Identification
k2   =  -0.1896;
k4   =  1.6550;
k6   =  -0.4654;

% T2
a1=25.69;
b1=749.3;
mt1 = 1;
bt1 = 29.04;
kt1 = 752.7;
Ps1 = k2*tf([a1 b1],[mt1 bt1 kt1]);

% T4
a2=65.99;
b2=1679;
mt2 = 1;
bt2 = 72.97;
kt2 = 1723;
Ps2 = k4*tf([a2 b2],[mt2 bt2 kt2]);

% T6
a3=63.77;
b3=6564;
mt3 = 1;
bt3 = 95.39;
kt3 = 6513;
Ps3 = k6*tf([a3 b3],[mt3 bt3 kt3]);

Gs = Ps1+Ps2+Ps3;

%Filter
[b,a] = butter(2,5/(30/2));
%sampled at 30Hz with a cuttoff of 5Hz
Hs = tf(b,a);

if strcmp('closed',ClosedorOpen)
    if nargin>1
        TF = (Gs*Ys)/(1+(Gs*Ys*Hs*Zeq)); % total system CL transfer function with impedance and A
    elseif nargin==1
        TF = Ys*Gs*Hs; % Total system CL transfer function without impedance feedback loop
    elseif nargin==0
        TF = Gs; % Total system CL transfer function without admittance controller or impedance feedback loop
    end
    
elseif strcmp('open',ClosedorOpen)
    if nargin>1
        TF = Ys*Gs*Hs*Zeq; % Open loop transfer function with impedance and A
    end
end


end