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

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_force = [];


curr_reading = inputSingleScan(s);

% Start long move (from current position to maximum indent position)
span_move = contact_end_pos - start_pos;
fprintf('Span Move = %f\n',span_move);
PI.PI_C863.MoveRelative('X',span_move); % start movement...

curr_pos = start_pos;
% Start stopwatch timer - there are two options how to implement time count
tic; % Start timer...
% t0 = 0; % Optional, depending on the time count method you use

while( abs(curr_pos) < (start_pos + span_move) ) % move stage until maximum indent reached
    
    % *********************************************************************
    % YOUR CODE HERE... 
    
    
    
    
    % somewhere in your code, ...
    % you will need Elapsed Time value to calculate displacement
    time_elapse = toc; % Elapsed time from the moment timer started
    
    
    
    
    % YOUR OTHER CODE HERE... if needed
    curr_pos = 0;       % CHANGE this
    curr_reading = 0;   % CHANGE this
    
    
    
    
    % *********************************************************************
    
	% Display current sensor reading
	fprintf('Sensor reading= %f\n', curr_reading);
    % Save the measurement data
    DATA_disp = [DATA_disp; curr_pos]; % Let just get raw data
    DATA_force = [DATA_force; curr_reading]; % We will remove offset later on in Linear Fit
    
    % tic; % Optional, depending on the time count method you use
    
end

time_elapse = toc; % ... stop timer

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

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

plot(DATA_disp, DATA_force, '.')

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

%% End