% TOGAC19_MSD_StabilityofAydinetal.m

% This file takes 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 and the stability of the system is analyzed using phase
% margin and closed loop poles

%%% It has been acknowledged that CL poles should be analyzed using a
%%% different method, and the method in this file is no longer valid. See
%%% the following files for a correct stability analysis:


% TOGAC24_MSD_TrueAydinetalStability.m
% Impedance_select.m
% TFAydin.m
% AydinStabMap.m
% CostTransMapAydin.m
% Check_stable.m

% Created by Brevin Banks
% Modified 3/13/2023

%default values

%human impedance
mh = 5;%5; Worst case reported mass from Aydin et al.
bh = 41;%41; Worst case reported damping from Aydin et al.
kh = 401;%401; Worst case reported stiffness from Aydin et al.

%environoment impedance
me = 0;%unused assumed only largely stiff
be = 0;%unused assumed only largely stiff
ke = 16599;% Worst case reported stiffness from Aydin et al.

m_ad = 50; % Desired impedance mass
b_ad = 500; % Desired impedance damping

Zhs = tf([mh bh],[1]) + tf([kh],[1 0]); % Impedance human
Ze = tf([me be],[1])+tf([ke],[1 0]); % Impedance from environment
Z = Zhs+Ze;

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

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

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

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

% The admittance controller
Ys = tf([1],[m_ad b_ad]);

% The resultant plant
Gs = Ps1+Ps2+Ps3;

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

fprintf('The system transfer function is: \n');
T = (Gs*Ys)/(1+(Gs*Ys*Hs*Z)) % total system CL transfer function
figure('name','Bode Plot of Aydin et al CL Transfer Function')
bode(T)
title('The resonse of the CL transfer function From Aydin et al.')

T = Ys*Gs*Hs*Z;% total system OL transfer function
figure('name','Bode Plot of Aydin et al OL Transfer Function')
bode(T)
title('The resonse of the OL transfer function From Aydin et al.')

%% Test for stability with phase margin and then closed loop poles

% Step size/refinement for testing stability is 60. 
unstablemabas = [0;0]; % record the values for which ma and ba make the system unstable
stable_stienen=ones(60,60); % initiate map for phase margin tests, a map of ones
stable_aydin=ones(60,60); % initiate map for closed loop tests, a map of ones
ma_values = linspace(1,200,60); % mass lin space from 1 to 200 with 60 steps
ba_values = linspace(1,1000,60);% damping lin space from 1 to 1000 with 60 steps

for masses = 1:length(ma_values)
    for dampers = 1:length(ba_values)
        m_ad = ma_values(masses);% assign the mass admittance values
        b_ad = ba_values(dampers);% assign the damping admittance values

        % The admittance controller
        Ys = tf([1],[m_ad b_ad]);

        Ya = Ys*Gs*Hs; % Open loop transfer function no impedance

        % Stienen et al. stability method by evaluating the phase margin

        % grab the phase margin of the system Ya*Zs
        [mag,phase1] = margin(Ya*Z);

        % Check if stable or not
        if isinf(phase1) % Stable if infinite
            stable_stienen(masses,dampers) = true;
        elseif phase1 <= 0 % unstable if negative
            stable_stienen(masses,dampers) = false;
        elseif phase1 > 0 % stable if positive
            stable_stienen(masses,dampers) = true;
        end


        % Aydin et al. stability method by analysing closed loop poles

        T = (Gs*Ys)/(1+(Gs*Ys*Hs*Z)); % total system CL transfer function
        poles = pole(T);

        for tester=1:length(poles)
            if real(poles(tester))>0
                stable_aydin(masses,dampers) =0;
                if unstablemabas(1,end)==m_ad
                    unstablemabas(1:2,end) = [m_ad;b_ad];
                else
                    unstablemabas(1:2,end+1) = [m_ad;b_ad];
                end
            end
        end
    end
end


stable_string = stringstable(stable_stienen);
fprintf("Stability according to Phase Margin\n"+stable_string)
fprintf('%d\n',poles);


formatSpec = 'Stability according to CL Poles: %s with the following CL poles \n';
stable_string = stringstable(stable_aydin);
fprintf(formatSpec,stable_string)
fprintf('%d\n',poles);



% This function will check the boolean case for the stable_condition
% variable. Because the above stability testers initialize the map with
% ones, but the stability checks return 1 for unstable, this will flip the
% response to return the correct variable to place in the map.
% Input:
%   stable_condition - a boolean for the stability of a TF for a given ma
%   and ba
function stable_string = stringstable(stable_condition)
if stable_condition==true
    stable_string = 'Stable';
else
    stable_string = 'Unstable';
end
end