clear

% Joystick script for controlling Xeryon stages.
% This script makes use of the "vrjoystick" command.
% See: https://nl.mathworks.com/help/sl3d/vrjoystick.html
% Step 1. Open our Windows interface, press "Connect" and then close the
% windows interface. 
% Step 2. Fill in the configurations below
COM_PORT = "COM1";
BAUDRATE = 115200;

MAX_SPEED_AXIS1 = [2000,-2000]; % The joystick will interpolate between these value's
MAX_SPEED_AXIS2 = [2000,-2000]; % expressed in mm/deg
AXIS_LETTERS = ["X", "Y"]; % Letter for axis 1 & axis 2
SPEED_MULTIPLIER1 = 1000; % 1000 for a linear stage / 100 for a rotating stage
SPEED_MULTIPLIER2 = 1000; % 1000 for a linear stage / 100 for a rotating stage
FLIP_1 = 1; % Flip axis, fill in 1 or -1 to flip the sign. 
FLIP_2 = -1;

% DEADZONE's
% If the joystick doesn't center well, the stage can keep moving. 
% To avoid this, the output value's of the joystick between the range of
% "DEADZONE" are put to 0. 
DEADZONE1 = [0.1,-0.1]; % The value's are percentages of the full range
DEADZONE2 = [0.1, -0.1];

% You can find the properties of the joystick that is connected to the 
% system in the Game Controllers section of the system Control Panel.
JOYSTICK_ID = 1; 

% Connect the joystick
joy = vrjoystick(JOYSTICK_ID);

% Read IO
% --------------------------------------------------------------------
% Setting up the serial connection to the Xeryon controller
serial = serialport(COM_PORT,BAUDRATE);


while 1
    [axes, buttons, povs] = read(joy);    
   
    if length(axes) >= 2
        % axes(1) returns a value between -1 and 1
        % Interpolate: O - Omin = (Omax-Omin)/(Imax-Imin)*(I-Imin) 
        
        % CALCULATIONS FOR AXIS 1
        % \__   1. Interpolate: (MAX_SPEED - 0)/(1 - max(DEADZONE)) * (value - max(DEADZONE)) + 0     
        %    \  2. deadzone ==> 0
        %       3. Interpolate: (MIN_SPEED - 0)/(-1 - min(DEADZONE)) *
        %       (value - min(DEADZONE)) + 0
        axis1_value = axes(1)*FLIP_1;
        
        if axis1_value >= max(DEADZONE1)
            % Above positive side of deadzone
            interp_axis1 = (max(MAX_SPEED_AXIS1) - 0)/(1-(max(DEADZONE1)))*(axis1_value-max(DEADZONE1)) + 0;
        elseif axis1_value <= min(DEADZONE1)
            % Below negative side of deadzone
            interp_axis1 = (min(MAX_SPEED_AXIS1) - 0)/(-1-(min(DEADZONE1)))*(axis1_value-min(DEADZONE1)) +0;
        else
            % Inside deadzone
            interp_axis1 = 0;
        end
        
        speed_axis1 = interp_axis1*SPEED_MULTIPLIER1;
        writeline(serial,string(AXIS_LETTERS(1)) + ":SSPD=" + string(speed_axis1));
        writeline(serial,string(AXIS_LETTERS(1)) + ":SCAN=" + string(sign(speed_axis1)));
        
        
        % CALCULATIONS FOR AXIS 2
        axis2_value = axes(2)*FLIP_2;
        
        if axis2_value >= max(DEADZONE2)
            % Above positive side of deadzone
            interp_axis2 = (max(MAX_SPEED_AXIS2) - 0)/(1-(max(DEADZONE2)))*(axis2_value-max(DEADZONE2)) + 0;
        elseif axis2_value <= min(DEADZONE2)
            % Below negative side of deadzone
            interp_axis2 = (min(MAX_SPEED_AXIS2) - 0)/(-1-(min(DEADZONE2)))*(axis2_value-min(DEADZONE2)) +0;
        else
            % Inside deadzone
            interp_axis2 = 0;
        end
        
        speed_axis2 = interp_axis2*SPEED_MULTIPLIER2;
        writeline(serial,string(AXIS_LETTERS(2)) + ":SSPD=" + string(speed_axis2));
        writeline(serial,string(AXIS_LETTERS(2)) + ":SCAN=" + string(sign(speed_axis2)));
    end
    pause(0.01) %in seconds
end

delete(serial)
