Skip to content

Instantly share code, notes, and snippets.

@lnrsoft
Forked from alexymik/XR2.control.m
Created February 9, 2018 22:06
Show Gist options
  • Save lnrsoft/b61c62822649aaf6310274c36578f0ed to your computer and use it in GitHub Desktop.
Save lnrsoft/b61c62822649aaf6310274c36578f0ed to your computer and use it in GitHub Desktop.
Matlab code to control XR-2 Robotic Arm with an Xbox 360 controller
% 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);
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment