%% FORCE MEASUREMENT
%  Laboratory Course
% =========================================================================
%  Incremental Step
% -------------------------------------------------------------------------
%  GROUP NUMBER: (put your group number here)
%  GROUP MEMBERS:
%  - (list name of group's member here)
%  - (list name of group's member here)
%  - (list name of group's member here)
%  - (add extra line as necessary)
% -------------------------------------------------------------------------

%% Preparation
% -------------------------------------------------------------------------

% Select correct settings for each sample
% Note: ****** Setting wrong parameters will result in HARDWARE DAMAGE! ******

% Sample 1 - Ecoflex
contact_start_pos = 5.8; 
contact_end_pos = 8.5;   

% Sample 2 - PDMS
%contact_start_pos = 5.2; 
%contact_end_pos = 5.5;  

% Sample 1 - EVA
%contact_start_pos = 5.2; 
%contact_end_pos = 5.36;   

offsetback = 0.1; % offset backward by X mm to not contact
velocity = 1;
PI.PI_C863.SetVelocity('X', velocity);

curr_pos = PI.PI_C863.GetPosition('X');
PI.PI_C863.Move('X',contact_start_pos - offsetback); 
pause( (abs(curr_pos-(contact_start_pos - offsetback))/velocity) + 1 );
start_pos = PI.PI_C863.GetPosition('X');
fprintf('Start at = %f\n', start_pos);

%% Measurement
% -------------------------------------------------------------------------

% Select correct settings for each sample

% Sample 1 - Ecoflex
velocity = 0.1; % mm/s
step_size = 0.1; % mm

% Sample 2 - PDMS
%velocity = 0.1; % mm/s
%step_size = 0.01; % mm

% Sample 3 - EVA
%velocity = 0.1; % mm/s
%step_size = 0.01; % mm

PI.PI_C863.SetVelocity('X', velocity);
wait_time = step_size / velocity;

% Variables to save the data
DATA_disp = [];
DATA_force1 = []; % after 1st delay
DATA_force2 = []; % after 2nd delay
DATA_force3 = []; % after 3rd delay
DATA_force4 = []; % after 4th delay, if needed

curr_reading = inputSingleScan(s);
curr_pos = start_pos;

while( abs(curr_pos) < (contact_end_pos) );
    
    % *********************************************************************
    % YOUR CODE HERE... if needed
    PI.PI_C863.MoveRelative('X',step_size);
    WaitMillisecond(wait_time*1000); 
    
    curr_pos = 0;  % Revise this line to get the stage position
    curr_reading = 0; % Revise this line to read force sensor output
    
    fprintf('(%f; %f)\n',curr_pos,curr_reading);
    
    DATA_force1 = [DATA_force1; curr_reading];
    
    WaitMillisecond(1000); % = 1 (second) 
    
    % Modify the code to record the force sensor output (with 1 second interval) when the sample position is the same
    % ADD CODE AS NECESSARY
    % *********************************************************************
    
    DATA_disp = [DATA_disp; curr_pos]; 
    
end

curr_pos = PI.PI_C863.GetPosition('X');
fprintf('Max bend at = %f\n', curr_pos);

PI.PI_C863.SetVelocity('X', 1);
PI.PI_C863.Move('X',0);

%% Save and plot the data
save('SingleRead_IncrementalStep', 'DATA_disp', 'DATA_force1', 'DATA_force2', 'DATA_force3', 'DATA_force4');

plot(DATA_disp, DATA_force1, '.')
hold on
plot(DATA_disp, DATA_force2, '+')
plot(DATA_disp, DATA_force3, '^')
plot(DATA_disp, DATA_force4, 's')
hold off

% End