%% FORCE MEASUREMENT
%  Laboratory Course
% =========================================================================
%  Single Read and 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
% -------------------------------------------------------------------------

contact_start_pos = 0;  % TO BE REPLACED with expected contact position
contact_end_pos = 0;    % TO BE REPLACED with expected max indent position

offsetback = 0.2; % offset backward by X mm to not contact
velocity = 1;

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

velocity = 0.010; % 0.01 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;

% Start moving stage step by step: do step, wait until step move is completed, do measurements
while( abs(curr_pos) < (contact_end_pos) ) % Move until maximum indentation position reached
    
    % *********************************************************************
    % YOUR CODE HERE... if needed
    curr_pos = 0;       % CHANGE this
    curr_reading = 0;   % CHANGE this
    
    
    
    
    fprintf('Sensor reading= %f\n', curr_reading);
    DATA_force1 = [DATA_force1; curr_reading];
    % ADD AS NECESSARY
    % *********************************************************************
    DATA_disp = [DATA_disp; curr_pos]; % Save raw data
    
end

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

% Save 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

% return stage to safe position
PI.PI_C863.SetVelocity('X', 1); 
PI.PI_C863.Move('X',9); 

%% End