Matlab code to control XR-2 Robotic Arm with an Xbox 360 controller

XR2.control.m

% Values you will most likely want to change: 
ROBOT_STEPS = 20;
COM_PORT = 'COM9';
POLL_RATE = 0.1;

% Set up joystick
disp('Connecting to Xbox Controller... ');
XBOX_CONTROLLER = vrjoystick(1);
disp(caps(XBOX_CONTROLLER));
%disp(INSTRFIND);

% Arm Link Mapping
% Check the motor controller to make sure these are in fact plugged in the
% way the code expects

% Link 1 - E
ROBOT_LINK_1 = 'e';
% Link 2 - G
ROBOT_LINK_2 = 'g';
% Link 3 - C
ROBOT_LINK_3 = 'c';
% Wrist Rotate - B
ROBOT_WRIST = 'b';
% Gripper - F
ROBOT_GRIPPER = 'f';


% From the robot arm documentation, the following serial settings are used:
% 9600 Baud, 7 Data Bits, 2 Stop Bits, Even Parity, Carriage Return for
% Terminator

disp(strcat('Connecting to Robot Arm on', {' '}, COM_PORT, '...'));
ROBOT_ARM = serial(COM_PORT,'BaudRate',9600,'DataBits',7,'StopBits',2,'Parity','Even','Terminator','CR');

% Open the serial connection
fopen(ROBOT_ARM);

% Xbox 360 controller mapping

% 1 - A Button
% 2 - B Button
% 3 - X Button
% 4 - Y Button
% 5 - L Bumper
% 6 - R Bumper
% 7 - Back Button
% 8 - Start Button
% 9 - LS3
% 10 - RS3

% Note that on some controllers there is a lot of junk input.
% Suggested to implement a deadzone, I use .5 and round to 
% the nearest whole.

% Axis 1 - Left Stick - Horizontal - Left -1 - Right 1
% Axis 2 - Left Stick - Vertical - Up -1 - Down 1
% Axis 3 - Triggers - Left Trigger ~1 - Right Trigger ~-1
% Axis 4 - Right Stick - Horizontal - Left -1 - Right 1
% Axis 5 - Right Stick - Vertical - Up -1 - Down 1


disp('Program started. Use Xbox Back Button or CTRL^C to end');
% Keep program running until back button is pressed
while (button(XBOX_CONTROLLER, 7) ~= 1)
    % Uncomment for debug
    %disp(axis(joy, 1));
    %disp(axis(joy, 2));
    %disp(axis(joy, 3));
    %disp(axis(joy, 4));
    %disp(axis(joy, 5));
    
    % Store instantaneous axis reading, as it is always changing
    XBOX_AXIS_1 = axis(XBOX_CONTROLLER, 1);
    
    % Check if the value is beyond the deadzone, absolute value is used as
    % a shortcut to check both axis directions at once
    if (abs(XBOX_AXIS_1) > .5)
       %disp(round(XboxAxis1)); 
       
       % Write the first part of serial command, select motor
       fprintf(ROBOT_ARM, '%c', ROBOT_LINK_1);
       % Round to -1 or 1 to indicate direction then multiply it by the
       % number of steps defined above
       fprintf(ROBOT_ARM, '%i\n', round(XBOX_AXIS_1) * ROBOT_STEPS);
    end
    
    
    XBOX_AXIS_2 = axis(XBOX_CONTROLLER, 2);
    if (abs(XBOX_AXIS_2) > .5) 
       fprintf(ROBOT_ARM, '%c', ROBOT_LINK_2);
       fprintf(ROBOT_ARM, '%i\n', round(XBOX_AXIS_2) * ROBOT_STEPS * -1);
    end
    
    
    XBOX_AXIS_3 = axis(XBOX_CONTROLLER, 3);
    if (abs(XBOX_AXIS_3) > .5) 
       fprintf(ROBOT_ARM, '%c', ROBOT_GRIPPER);
       fprintf(ROBOT_ARM, '%i\n', round(XBOX_AXIS_3) * ROBOT_STEPS);
    end
    
    
    XBOX_AXIS_5 = axis(XBOX_CONTROLLER, 5);
    if (abs(XBOX_AXIS_5) > .5) 
       fprintf(ROBOT_ARM, '%c', ROBOT_LINK_3);
       fprintf(ROBOT_ARM, '%i\n', round(XBOX_AXIS_5) * ROBOT_STEPS);
    end
    
    if (button(XBOX_CONTROLLER, 5) == 1)    
       fprintf(ROBOT_ARM, '%c', ROBOT_WRIST);
       fprintf(ROBOT_ARM, '%i\n', ROBOT_STEPS);
    end
    
    if (button(XBOX_CONTROLLER, 6) == 1)    
       fprintf(ROBOT_ARM, '%c', ROBOT_WRIST);
       fprintf(ROBOT_ARM, '%i\n', ROBOT_STEPS * -1);
    end
    
    pause(POLL_RATE);
end
disp('Terminating program...');

fclose(ROBOT_ARM);

close(XBOX_CONTROLLER);