%% TOGAC12_MSD_MassSpringDamperPlayground.m

% This file contains debugging sections of code used in the math and
% development of transfer functions and their stability analysis for the
% admittance controller design for the galen robot. No functions or
% sections in this code are used in the implimentation of the controller or
% the proper figures used to plot the stability or transparency. This code
% was soley used for debugging, and may potentially contain out of date
% versions of transfer functions or equations.

% The mass spring damper introduced here is the mass spring damper used in
% any other MSD documents except for the Aydin or Steinen functions or
% files that explicitly state they are using different control values.

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

%% Section 1 Fixed System Properties
clear all
close all

m = 10; %kg System mass for MSD Ps
b = 5; %Ns/m System damping for MSD Ps
k = 1; %N/m System stiffness for MSD Ps
% the following control values were used from 2/23/2023 to 3/7/2023
P = 900; %Ns/m Proportional gain for controller Cs
D = 10; % Derivative gain for controller Cs
% the following control values were used from 3/7/2023 on for MSD
% simulation
P = 4357.72516429453; %Ns/m Proportional gain for controller Cs 
D=10; % Derivative gain for controller Cs

N = 1000; % Simulative derivative delay for a low pass derivtive filter. Unused in analytical results
I = 0; %N/m Integral gain for controller Cs (unused currently)
muff = 10; %kg Feed forward mass derivative gain (close to the Plant mass)
bff = 2; %Ns/m Feed forward damping derivative gain (close to the Plant damping)

%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 = 5; %kg admittance mass to play around with, can vary
b_ad = 0; %Ns/m admittance damping to play around with, can vary, preferably 0

%% Section 2 Playground Values for quick controller simulating. Update freely
% Human impedance
mh = 5;
bh = 41;
kh = 401;
% Environment impedance
me = 0;
be = 0;
ke = 16599;
% Admittance values
m_ad = 5; %kg admittance mass to play around with, can vary
b_ad = 0; %Ns/m admittance damping to play around with, can vary, preferably 0

fprintf('\nDone updating parameters\n\n')

%% Section 3 Impedance measurments styled as like Steinen paper (No longer relevant)
%  This section Will isolate the tf Fh to V for impedance measurements
%  and plot bode plots of the result for different MSPDPlaygrounds
%  MSPDPlaygrounds are variations of the mass spring damper controller
%  with position or velocity control, feedback, and feedforward plant
%  linearization. MSPD8playground is no longer as listed here and features
%  new controls but the current MSPD8Playground is the current canonical
%  desired control scheme

masses = [0.05 0.5 1 3 5 8 10]; % Arbitrary range of masses to test with
close all
for k=1:length(masses)
    m_ad = masses(k);
    %MSPDPlayground 2 Impedance Position only
    As = tf([1],[m_ad 0 0]);
    Cs = tf([P+D*N N*P+I I*N],[1 N 0]);
    Ps = tf([1],[m b k]);
    dZs2 = (1+Cs*Ps)/(As*Cs*Ps)
    [num den] = tfdata(dZs2);
    AdZs2 = tf([den],[num]);
    C_map2 = transparency_map(1,Ps,Cs)
    
    %MSPDPlayground 3 Impedance Position with pass through
    As = tf([1],[m_ad 0 0]);
    Cs = tf([P+D*N N*P+I I*N],[1 N 0]);
    Ps = tf([1],[m b k]);
    dZs3 = (1+Cs*Ps)/(As*Cs*Ps+Ps)
    [num den] = tfdata(dZs3);
    AdZs3 = tf([den],[num]);
    C_map3 = transparency_map(2,Ps,Cs)
    
    %MSPDPlayground 4 Impedance Velocity only
    As = tf([1],[m_ad 0]);
    Cs = tf([5 900],[1]);
    Ps = tf([1],[m b]);
    dZs4 = (1+Cs*Ps)/(As*Cs*Ps)
    [num den] = tfdata(dZs4);
    AdZs4 = tf([den],[num]);
    C_map4 = transparency_map(3,Ps,Cs)
    
    %MSPDPlayground 5 Impedance Position with Linearized Dynamics
    As = tf([1],[m_ad 0 0]);
    Cs = tf([P+D*N N*P+I I*N],[1 N 0]);
    Ps = tf([1],[m 0 0]);
    dZs5 = (1+Cs*Ps)/(As*Cs*Ps)
    [num den] = tfdata(dZs5);
    AdZs5 = tf([den],[num]);
    C_map5 = transparency_map(1,Ps,Cs)
    
    %MSPDPlayground 6 Impedance Position with pass through and Linearized Dynamics
    As = tf([1],[m_ad 0 0]);
    Cs = tf([P+D*N N*P+I I*N],[1 N 0]);
    Ps = tf([1],[m 0 0]);
    dZs6 = (1+Cs*Ps)/(As*Cs*Ps+Ps)
    [num den] = tfdata(dZs6);
    AdZs6 = tf([den],[num]);
    C_map6 = transparency_map(2,Ps,Cs)
    
    %MSPDPlayground 7 Impedance Velocity with pass through and Linearized Dynamics
    As = tf([1],[m_ad 0]);
    Cs = tf([5 900],[1]);
    Ps = tf([1],[m 0]);
    dZs7 = (1+Cs*Ps)/(As*Cs*Ps+Ps)
    [num den] = tfdata(dZs7);
    AdZs7 = tf([den],[num]);
    C_map7 = transparency_map(4,Ps,Cs)
    
    %Impedance Velocity with pass through
    As = tf([1],[m_ad 0]);
    Cs = tf([5 900],[1]);
    Ps = tf([1],[m b]);
    dZsVPT = (1+Cs*Ps)/(As*Cs*Ps+Ps)
    [num den] = tfdata(dZsVPT);
    AdZsVPT = tf([den],[num]);
    
    %Impedance Velocity with Linearized Dynamics
    As = tf([1],[m_ad 0]);
    Cs = tf([5 900],[1]);
    Ps = tf([1],[m 0]);
    dZsVLD = (1+Cs*Ps)/(As*Cs*Ps)
    [num den] = tfdata(dZsVLD);
    AdZsVLD = tf([den],[num]);
    
    % Plot the various Amdittances on a bode plot for position
    str1 = 'Admittance Bode Plots for m_ad = ';
    str2 = string(m_ad);
    figure('name',str1+str2+' Position')
    hold on
    bode(AdZs2,'b--')
    bode(AdZs3,'b*')
    bode(AdZs5,'b.')
    bode(AdZs6,'b')
    bode(tf([1],[m_ad 0 0]),'m'); %Position As
    bode(tf([1],[m b 0]),'g'); %Position Ps
    xlim([10^-2 10^3])
    legend('Position Only','Position W/Pass Through','Position W/LinDyn','Position W/LinDynPassThrough','A(s)','P(s)','location','bestoutside')
    title('Position Bode Plot of 1/\DeltaZ(s), Admittance with m_{ad}='+str2)
    str3 = 'P_Admit_m '+str2+'.png';
    str4 = 'P_Admit_m '+str2+'.fig';
    
    % Plot the various Amdittances on a bode plot for velocity
    figure('name',str1+str2+' Velocity')
    hold on
    bode(AdZs4,'r--')
    bode(AdZsVPT,'r*')
    bode(AdZsVLD,'r.')
    bode(AdZs7,'r')
    bode(tf([1],[m_ad 0]),'m'); %Velocity As
    bode(tf([1],[m b]),'g'); %Velocity Ps
    xlim([10^-2 10^3])
    legend('Velocity Only','Velocity W/PassThrough','Velocity W/LinDyn','Velocity W/LinDynPassThrough','A(s)','P(s)','location','bestoutside')
    title('Velocity Bode Plot of 1/\DeltaZ(s), Admittance with m_{ad}='+str2)
    str3 = 'V_Admit_m '+str2+'.png';
    str4 = 'V_Admit_m '+str2+'.fig';
    
end
%% Section 4 Arvid and Steinen Admittance Control design
%This section focuses on impedance for more specialized controllers
% Modeled after the Arvid and Steinen Admittance controller (2018)

masses = [0.05 0.5 1 3 5 8 10]; % Arbitrary range of masses to test with
close all
for k = 1:length(masses)
    m_ad = masses(k);
    str1 = string(m_ad);
    %Impedance Velocity from ArvidAdmittanceController, Cfb mspS and Pass Through
    As = tf([1],[m_ad b_ad]);
    Cfb = kr*tf([I],[1 0]) + tf([P],[1]) + tf([D 0],[1/N 1]);
    Cff = tf([muff bff],[1]);
    Ps = tf([1],[m b]);
    Zps = tf([mps 0],[1]); % Impedance from robot inertia
    Yabasline = (Ps*(1 + Cfb*As*kr))/(Ps*(Zps + Cfb*(kr + As*Zps*kr)) + 1); % Closed loop tf from Fext to V
    Yapi = tf([1],[m 0]);
    
    figure('name','ArvidAdmittanceController m='+str1)
    hold on
    bode(Yabasline,'b')
    bode(Yapi,'b--')
    bode(tf([1],[m_ad b_ad]),'m'); %Position As
    bode(tf([1],[m b]),'g'); %Position Ps
    xlim([10^-2 10^3])
    title('Bode for ArvidAdmittanceController m_{ad}='+str1)
    legend('Ya','Passive Ya','A(s)','P(s)')
    C_map_Arvid = Comp_transparency_map(1,Ps,Cfb,Cff,Gf,Zps,kr)
    
    %Impedance Velocity from OldMSPD8Playground, Cfb Cff Gf mspS and Pass Through
    As = tf([1],[m_ad b_ad]);
    Cfb = kr*tf([I],[1 0]) + tf([P],[1]) + tf([D 0],[1/N 1]);
    Cff = tf([muff bff],[1]);
    Ps = tf([1],[m b]);
    Zps = tf([mps 0],[1]); % Impedance from robot inertia
    Yabasline = (Ps*(1 + Cfb*As*kr))/(Ps*(Zps + Cfb*(kr + As*Zps*kr)) + 1); % Closed loop tf from Fext to V
    dZsV8 = (Ps*(Zps + Gf*Zps + (kr + As*Zps*kr)*(Cfb + Cff)) + 1)/(Ps*(1 + Gf + As*kr*(Cfb + Cff)))
    [num den] = tfdata(dZsV8);
    AdZsVLD = tf([den],[num]);
    
    figure('name','MSPD8Playground m='+str1)
    hold on
    bode(AdZsVLD,'b')
    bode(Yabasline,'b--')
    bode(tf([1],[m_ad b_ad]),'m'); %Position As
    bode(tf([1],[m b]),'g'); %Position Ps
    xlim([10^-2 10^3])
    title('Bode for MSPD8Playground m_{ad}='+str1)
    legend('Ya','Ya no Cff','A(s)','P(s)')
    C_map_MSPD8 = Comp_transparency_map(2,Ps,Cfb,Cff,Gf,Zps,kr)
end

%% Funciton block

%This transparency map function works in conjunction with the code in section 3
function C_map = transparency_map(kind,Ps,Cs)
i = 0;
j = 0;

fs = 296; % samples in 10 seconds
fc = 5; % cut off frequency
[b,a] = butter(5,fc/(fs/2)) ;
cost_filt = tf(b,a);
for m_ad = 0.01:1:30
    i = i+1;
    for b_ad = 0.01:1:100
        j = j+1
        switch kind
            case 1 % No pass through position
                As = tf([1],[m_ad b_ad 0]);
                dZs = (1+Cs*Ps)/(As*Cs*Ps);
            case 2 % Pass through position
                As = tf([1],[m_ad b_ad 0]);
                dZs = (1+Cs*Ps)/(As*Cs*Ps+Ps);
            case 3 % No pass through velocity
                As = tf([1],[m_ad b_ad]);
                dZs = (1+Cs*Ps)/(As*Cs*Ps);
            case 4 % Pass through velocity
                As = tf([1],[m_ad b_ad]);
                dZs = (1+Cs*Ps)/(As*Cs*Ps+Ps);
        end
        space = logspace(-2,2);
        space = space(1:45);
        C = 0;
        for k = 1:length(space)
            [dZsmag,psZ] = mag_phase(dZs,space(k));
            [Weightfun,psW] = mag_phase(cost_filt,space(k));
            C = C + Weightfun*log(dZsmag);
        end
        
        C_map(i,j) = C;
    end
    j = 0;
end

%plot the cost map
str1 = 'transparency for type ';
str2 = string(kind);
str3 = str1 + str2;
figure('name',str3)
imagesc([0.01:1:100],[0.01:1:30],C_map)
caxis([0 90])
set(gca,'YDir','normal')
colorbar
xlabel({'b [Ns/m]';'W(\omega) is a weight function where it is the magnitude of a butterworth filter at 5Hz'})
ylabel('m [kg]')
title({'Parasitic Impedance Cost Map';'Beurger and Hogan (2007) \Sigma_{0.01}^{30Hz} W(\omega)\Delta|Z(jw)|'})
end
%This transparency map function works in conjunction with the code in
%section 4
function C_map = Comp_transparency_map(kind,Ps,Cfb,Cff,Gf,Zps,kr)
i = 0;
j = 0;

fs = 296; % samples in 10 seconds
fc = 5; % cut off frequency
[b,a] = butter(5,fc/(fs/2)) ;
cost_filt = tf(b,a);
for m_ad = 0.01:1:20
    i = i+1;
    for b_ad = 0.01:1:100
        j = j+1
        As = tf([1],[m_ad b_ad]);
        switch kind
            case 1 % Arvid and Steinen
                dZs = (Ps*(Zps + Cfb*(kr + As*Zps*kr)) + 1)/(Ps*(1 + Cfb*As*kr));
            case 2 % MSPD8Playground
                dZs = (Ps*(Zps + Gf*Zps + (kr + As*Zps*kr)*(Cfb + Cff)) + 1)/(Ps*(1 + Gf + As*kr*(Cfb + Cff)));
        end
        space = logspace(-2,2);
        space = space(1:45);
        C = 0;
        for k = 1:length(space)
            [dZsmag,psZ] = mag_phase(dZs,space(k));
            [Weightfun,psW] = mag_phase(cost_filt,space(k));
            C = C + Weightfun*log(dZsmag);
        end
        
        C_map(i,j) = C;
    end
    j = 0;
end

%plot the cost map
str1 = 'transparency for type ';
str2 = string(kind);
str3 = str1 + str2;
figure('name',str3)
imagesc([0.01:1:100],[0.01:1:20],C_map)
caxis([0 110])
set(gca,'YDir','normal')
colorbar
xlabel({'b [Ns/m]';'W(\omega) is a weight function where it is the magnitude of a butterworth filter at 5Hz'})
ylabel('m [kg]')
title({'Parasitic Impedance Cost Map';'Beurger and Hogan (2007) \Sigma_{0.01}^{30Hz} W(\omega)\Delta|Z(jw)|'})
end