% This function outputs a transfer function based on the impedance cases
% outlined in the paper Towards  Collaborative  Drilling  with  a 
% Cobot  Using  Admittance  Controller by Aydin et al. 2020
% These impedance bounds represent worst case human and environment
% interactions for the robot end.
%   Input:
%       case_num - an Int from 1 to 8 that selects the different impedance
%       cases. Look at each case in the switch statement to understand the
%       possible cases
%   Output:
%       Zeq - resultant impedance transfer function to multiply the output
%       velocity by before being fedback to the input force of the system

% Created by Brevin Banks 
% Modified 3/14/2023
function Zeq = Impedance_select(case_num)
if nargin==0
    case_num = 1;
end

switch case_num
    case 1 %Extreme Case set 1 full Ze and Zh
        mh = 5;
        bh = 41;
        kh = 401;
        me = 0;
        be = 0;
        ke = 16599;
        
    case 2 %Extreme Case set 2 full Ze and Zh with only mass
        mh = 5;
        bh = 0;
        kh = 401;
        me = 0;
        be = 0;
        ke = 16599;
    case 3 %Extreme Case set 3 full Ze no Zh mass
        mh = 0;
        bh = 41;
        kh = 401;
        me = 0;
        be = 0;
        ke = 16599;
    case 4 %Extreme Case set 4 full Ze no Zh mass or damping
        mh = 0;
        bh = 0;
        kh = 401;
        me = 0;
        be = 0;
        ke = 16599;
    case 5 %Extreme Case set 5 no Ze only Zh mass and damping
        mh = 5;
        bh = 41;
        kh = 401;
        me = 0;
        be = 0;
        ke = 0;
    case 6 %Extreme Case set 6 no Ze only Zh mass
        mh = 5;
        bh = 0;
        kh = 401;
        me = 0;
        be = 0;
        ke = 0;
    case 7 %Extreme Case set 7 no Ze only Zh damping
        mh = 0;
        bh = 41;
        kh = 401;
        me = 0;
        be = 0;
        ke = 0;
    case 8 %Extreme Case set 8 no Ze or Zh
        mh = 0;
        bh = 0;
        kh = 401;
        me = 0;
        be = 0;
        ke = 0;
end
s = tf('s');
Zhs = mh*s + bh + (kh/s);
Zes = me*s + be + (ke/s);

if is_equal_tf(Zes,tf([1],[1])*0)% if environment impedance is zero set the tf to 1
    Zes = 0;
end


Zeq = minreal(Zhs+Zes);
end