% TFFeedbackForward
% This file takes the pieces of the transfer function from
% our feedback including feedforward and feedforward linearization
% 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 = TFFeedbackForward(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;
muff = 10; %kg
bff = 2; %Ns/m

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


if strcmp('closed',ClosedorOpen)
    if nargin>1
        TF = Ps*(A*Cfb+A*Cff+1)/(Ps*Zeq*(A*Cfb+A*Cff+1)+Ps*Cfb+1);% Total system CL transfer function with impedance and A
    elseif nargin==1
        TF = Ps*(A*(Cfb+Cff)+1)/(1+Cfb*Ps); % Total system CL transfer function without impedance feedback loop
    elseif nargin==0
        TF = Ps*((Cfb+Cff)+1)/(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+Cff )+1)*Ps)*Zeq/(Ps*Cfb+1); % Open loop transfer function with impedance and A
    end
end

end