% TOGAC24_MSD_TrueAydinetalStability.m

% The following file takes the transfer function from the Aydin et al
% Collaborative drilling paper and analyzes stability of G(s) over the same
% range of ma and ba values identified in the paper's figure 6
% Note that the output response differs than that recorded in that paper.
% The maps appear to behave like ramps rather than the shoe horn curves
% observed in figures 6 and 7 of the paper. Further analysis was done to
% check if the individual transfer functions that make up G(s), that being
% T2, T4, and T6, were used to make the map assuming one of those 3
% transfer functions is simply a worst case. The maps still did not match
% that of figures 6 and 6, and the results did not return
% with a conlusive answer as to why the stability may be off. The results
% seem either wrongly reported in the Aydin et al paper, or our methods are
% incorrect. However the following method has been reviewed and validated
% with the galen AMBF model for stability analysis of that identified
% system.
% Created by Brevin Banks
% Modified 3/16/2023

% Create the space of ma and ba values to test the admittance controller on
ma_space = linspace(0.1,200,20);
ba_space = linspace(0.1,1000,20);

% select from a list of end case impedance values outlined in the Aydin et
% al paper. Then analyze the stability of the TF from Aydin et al with the
% given ma space, ba space, and impedance
Zeq = Impedance_select(1); 
[stable_map1, line_of_stability1] = AydinStabMap(ma_space,ba_space,Zeq)
Zeq = Impedance_select(2);
[stable_map2, line_of_stability2] = AydinStabMap(ma_space,ba_space,Zeq)
Zeq = Impedance_select(3);
[stable_map3, line_of_stability3] = AydinStabMap(ma_space,ba_space,Zeq)
Zeq = Impedance_select(4);
[stable_map4, line_of_stability4] = AydinStabMap(ma_space,ba_space,Zeq)

% extract the cost map for the Aydin controller using the Aydin et al
% method in section 5 Transparency. This should mirror figure 8
Cost_MapAydin = CostTransMapAydin(ma_space,ba_space);

% Stab mapper takes in Lines in the form of a cell array
Lines={line_of_stability1,line_of_stability2,line_of_stability3,line_of_stability4};

% Plot the stability plots and cost map in one figure
StabMapper(ma_space,ba_space,Lines,Cost_MapAydin)


%%% Lower bound impedances

% Create the space of ma and ba values to test the admittance controller on
ma_space = linspace(0.1,25,20);
ba_space = linspace(0.1,50,20);

% select from a list of end case impedance values outlined in the Aydin et
% al paper. Then analyze the stability of the TF from Aydin et al with the
% given ma space, ba space, and impedance
Zeq = Impedance_select(5);
[stable_map1, line_of_stability1] = AydinStabMap(ma_space,ba_space,Zeq)
Zeq = Impedance_select(6);
[stable_map2, line_of_stability2] = AydinStabMap(ma_space,ba_space,Zeq)
Zeq = Impedance_select(7);
[stable_map3, line_of_stability3] = AydinStabMap(ma_space,ba_space,Zeq)
Zeq = Impedance_select(8);
[stable_map4, line_of_stability4] = AydinStabMap(ma_space,ba_space,Zeq)

% extract the cost map for the Aydin controller using the Aydin et al
% method in section 5 Transparency. This should mirror figure 9
Cost_MapAydin = CostTransMapAydin(ma_space,ba_space);

% Stab mapper takes in Lines in the form of a cell array
Lines={line_of_stability1,line_of_stability2,line_of_stability3,line_of_stability4};

% Plot the stability plots and cost map in one figure
StabMapper(ma_space,ba_space,Lines,Cost_MapAydin)

