% TOGAC20_MSD_StabilityofFeedback.m

% This file takes the transfer function from the MSD feedback controller
% that provides the framework for our approach.
% The system stability 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:


% Impedance_select.m
% TFFeedback.m
% FeedbackStabMap.m
% FeedbackStabMapNoImped.m
% CostTransMapFeedback.m
% Check_stable.m

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


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

%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 = minreal(Zhs+Ze);

% 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

% P = tf([1],[m b]); % not spring stiffness on velocity. Similar to Steinen et al Yr

% controller gains
P = 900; %Ns/m
D = 10;
muff = 10; %kg
bff = 2; %Ns/m

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

A = tf([1],[m_ad b_ad]); % Admittance Controller


fprintf('The system transfer function is: \n');
T = (A*Cfb*Ps)/(1+Cfb*Ps*(A*Z+1)) % Total system CL transfer function


figure('name','Bode Plot of feedback only Transfer Function')
bode(T)
title('The resonse of the CL feedback only transfer function.')

%% Stienen et al. stability method Phase margin evaluation


Ya = (A*Cfb*Ps)/(Ps*Cfb+1); % Open loop transfer function no impedance

% 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 = true;
elseif phase1 <= 0 % unstable if negative
    stable_stienen = false;
elseif phase1 > 0 % stable if positive
    stable_stienen = true;
end

formatSpec = 'Stability according to phase margin: %s with phase margin of %4.2f \n';
stable_string = stringstable(stable_stienen);
fprintf(formatSpec,stable_string,phase1)

%% Aydin et al. stability method closed loop poles

stable_aydin =1;

poles = pole(T);
for tester=1:length(poles)
    if real(poles(tester))>0
        stable_aydin =0;
    end
end



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