% TFFeedback
% This file takes the pieces of the transfer function from
% our feedback controller and will output the desired form for either a closed loop or
% open loop function. It will also apply impedance if desired
% 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 = TFFeedback(A,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

%default values for a given mass spring damper
m = 10; %kg
b = 5; %Ns/m
k = 1; %N/m


% our plant design with velocity control of mass spring damper according to
% simulink tf extraction
Ps = tf([1 0],[m b k]); % dynamics tf including spring stiffness for velocity

% controller gains
P = 4357.72516429453;%900; %Ns/m
D = 10;%10;

Cfb = tf([D 0],[1]) + tf([P],[1]); %Feedback


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

end