
%% Purpose
% To show an example of the accompanying function 
% "ComputeSpringDamperRenderingStabilityAndQuantizationErrorLimits" can be 
% used to help answer research questions of rendering stiff dynamics using 
% a virtual spring damper for haptic displays. 
% In this example we generate three "renderable regions" in the virtual 
% stiffness and damping plane. These regions are:
% A1 - sampled data passive and quantization error passive
% B1 - uncoupled stable and quantization error passive
% B2 - uncoupled stable and quantization error active 
% (the letters correspond to stability conditions, and the numbers 
% correspond to quantization error conditions, as described in the paper). 
% For a point in a certain region, e.g., A1, there exists low-pass filter 
% control parameters that satisfy conditions for that region. 
% 
% "ComputeSpringDamperRenderingStabilityAndQuantizationErrorLimits" must 
% be included in the path for this file to run successfully. The code takes 
% approximately five minutes to run on a computer running Windows 7 
% using an Intel i5 processor. 

%% Author Information and Date
% Nick Colonnese
% February 10, 2015
% Department of Mechanical Engineering, Stanford University

%% Parameters of Haptic Display 
m = .250; % device mass (kg)
b = .5;   % device damping (Ns/m)
c = .1;   % device Coulomb friction (N)
delta = 0.05e-3; % position quantization interval (m)
T = 0.001; % sample time (s)
m_h = 0;   % human mass (kg)      
b_h = 0;   % human damping (Ns/m)
k_h = 0;   % human stiffness (N/m) 
delay = 3; 

%% Control Parameter Space 
f_0_range = linspace(10,500,50);  % (Hz)
B_range = linspace(0,20,20);      % (Ns/m)

%% Generate Renderable Regions 
Kmax_A1 = zeros(length(B_range),1); 
Kmax_B1 = zeros(length(B_range),1); 
Kmax_B2 = zeros(length(B_range),1); 

A1_controlParameters = []; 
B1_controlParameters = [];
B2_controlParameters = []; 

for i = 1:length(B_range) % over virtual daming values 
    % display progress 
    progressString = [num2str(i),'/',num2str( length(B_range) ), ' complete']; 
    disp(progressString); 
    B = B_range(i); 
    for n = 1:4 % over filter order 1-4
        for j = 1:length(f_0_range)
            f_0 = f_0_range(j); 
            
            % 1) compute maximum K's 
            results = ComputeSpringDamperRenderingStabilityAndQuantizationErrorLimits(...
                           m,b,c,B,delta,T,f_0,n,delay,m_h,b_h,k_h);

            % 2) update Kmax for regions 
            % A1
            if( min([results.K_sampledDataPassive results.K_quantizationErrorPassive]) > Kmax_A1(i) )
                Kmax_A1(i) = min([results.K_sampledDataPassive results.K_quantizationErrorPassive]);
            end
            % B1
            if( min([results.K_uncoupled results.K_quantizationErrorPassive]) > Kmax_B1(i) )
                Kmax_B1(i) = min([results.K_uncoupled results.K_quantizationErrorPassive]);
            end
            % B2
            if( min([results.K_uncoupled results.K_maliciousTouchLimitCycles]) > Kmax_B2(i) )
                Kmax_B2(i) = min([results.K_uncoupled results.K_maliciousTouchLimitCycles]);
            end
            
            % 3) store data for finding Kmax in region 
            A1_controlParameters = [A1_controlParameters; B, min([results.K_sampledDataPassive results.K_quantizationErrorPassive]) f_0 n ]; 
            B1_controlParameters = [B1_controlParameters; B, min([results.K_uncoupled results.K_quantizationErrorPassive]) f_0 n ]; 
            B2_controlParameters = [B2_controlParameters; B, min([results.K_uncoupled results.K_maliciousTouchLimitCycles]) f_0 n ];             
        end
    end
end

%% Find max K for each region, and control paramters to achieve it 
% A1
[KmaxRender_A1 Kmax_A1_index] = max(A1_controlParameters(:,2)); 
Kmax_A1_params = A1_controlParameters(Kmax_A1_index,:); 
% B1
[KmaxRender_B1 Kmax_B1_index] = max(B1_controlParameters(:,2)); 
Kmax_B1_params = B1_controlParameters(Kmax_B1_index,:); 
% B2
[KmaxRender_B2 Kmax_B2_index] = max(B2_controlParameters(:,2)); 
Kmax_B2_params = B2_controlParameters(Kmax_B2_index,:); 

%% Plot Parameters

color1 = [0 1 0];
color2 = [1 (51/255) (51/255)]; 
color3 = [0 0 80/255]; 

lineWidth = 3.0;

%% Plot
plot(B_range,Kmax_A1,'-','LineWidth',lineWidth,'Color',color3); 
hold on; 
plot(B_range,Kmax_B1,'-','LineWidth',lineWidth,'Color',color2);
plot(B_range,Kmax_B2,'-','LineWidth',lineWidth,'Color',color1);

grid on; 
xlim([0 20]);
ylim([0 2500])
xlabel('B')
ylabel('K')

set(gca,'YTick',(0:500:2500))
set(gca,'XTick',(0:5:20))

xlabel('Virtual Damping (Ns/m)');
ylabel('Virtual Stiffness (N/m)');
titleString = {'Renderable Regions',...
      '(the regions are below the curves)'}; 
title(titleString);
legend('A1 - Sampled data passive and quantization error passive',...
       'B1 - Uncoupled stable and quantization error passive',...
       'B2 - Uncoupled stable and quantization error active','Location','Best'); 
grid on;

