% TOGAC26_AMBF_StepIdentifiedSystemStab.m

% The following file takes the identified transfer function from the AMBF 
% Step response system: TF = (5262)/(s^2 + 138.6*s + 5265); for the galen
% robot system in simulation. This will produce 2 figures. A stability map
% with a cost map for the upper impedance cases (including the environment)
% and lower cases without the environement. Both upper and lower include
% human impedance cases.

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

% Recreate the identified transfer function for the system here. this is
% the result from tfest 2nd order approximation
s = tf('s');
TF = (5262)/(s^2 + 138.6*s + 5265);


% 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,1400,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] = AnyTFStabMap(ma_space,ba_space,TF,Zeq)
Zeq = Impedance_select(2);
[stable_map2, line_of_stability2] = AnyTFStabMap(ma_space,ba_space,TF,Zeq)
Zeq = Impedance_select(3);
[stable_map3, line_of_stability3] = AnyTFStabMap(ma_space,ba_space,TF,Zeq)
Zeq = Impedance_select(4);
[stable_map4, line_of_stability4] = AnyTFStabMap(ma_space,ba_space,TF,Zeq)

% extract the cost map for the Aydin controller using the Aydin et al
% method in section 5 Transparency. 
CostTF = CostTransMapAnyTF(ma_space,ba_space,TF);
% 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,CostTF)


%%% 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);

Zeq = Impedance_select(5);
[stable_map1, line_of_stability1] = AnyTFStabMap(ma_space,ba_space,TF,Zeq)
Zeq = Impedance_select(6);
[stable_map2, line_of_stability2] = AnyTFStabMap(ma_space,ba_space,TF,Zeq)
Zeq = Impedance_select(7);
[stable_map3, line_of_stability3] = AnyTFStabMap(ma_space,ba_space,TF,Zeq)
Zeq = Impedance_select(8);
[stable_map4, line_of_stability4] = AnyTFStabMap(ma_space,ba_space,TF,Zeq)

% extract the cost map for the Aydin controller using the Aydin et al
% method in section 5 Transparency. 
CostTF = CostTransMapAnyTF(ma_space,ba_space,TF);
% 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,CostTF)

