%% FORCE MEASUREMENT
%  Laboratory Course
% =========================================================================
%  Continous Move
% -------------------------------------------------------------------------
%  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 3 - EVA
%contact_start_pos = 5.2; 
%contact_end_pos = 5.36; 

offsetback = 0.1; % offset backward by X mm 
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.025; % mm

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

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

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

% Variables to save the data
DATA_disp = [];
DATA_force = [];

curr_reading = inputSingleScan(s);

% Start long move
span_move = contact_end_pos - start_pos;
fprintf('Span Move = %f\n',span_move);
PI.PI_C863.MoveRelative('X',span_move); % start the move...

curr_pos = start_pos;

%% Setting up the timer

% YOUR CODE HERE... if needed

while( abs(curr_pos) < (start_pos + span_move) ) % Termination condition: reaching to max indentation
    
   WaitMillisecond(wait_time*1000);
   
% YOUR CODE HERE... if needed
% Elapsed time is required to estimate the stage displacement
     
    curr_pos = 0;  % Revise this line to estimate the stage position based on timer
    curr_reading = 0;  % Revise this line to read force sensor output 
    
    fprintf('(%f; %f)\n',curr_pos, curr_reading);
% *********************************************************************
    
% Store the measurement data
    DATA_disp = [DATA_disp; curr_pos]; 
    DATA_force = [DATA_force; curr_reading]; 
   
   end

time_elapse = toc; 

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

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

%% Save and plot the data
save('SingleRead_ContinousMove', 'DATA_disp', 'DATA_force');
plot(DATA_disp, DATA_force, '.')

% End