Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
% Simplified source code for mecanum wheel base demo video without
% camera and tilt
%
% YouTube: Teleop Taxi - HEBI Robotics
% https://youtu.be/zaPtxre4tFc
%
% Features: joystick input to control motions of a 4 wheel base
%
% Requirements: MATLAB 2013b or higher
% Logitech Gamepad F310
%
% Author: Florian Enner
% Date: 28 March, 2016
% API: sdk0.3-rev1263
%
% Copyright 2016 HEBI Robotics
%% Setup
wheels = HebiLookup.newGroupFromNames('Cart', {
'X5-1_00004' % left front
'X5-1_003' % left back
'X5-1_004' % right front
'X5-1_001' % right back
});
joy = vrjoystick(1);
%% Control
cmd = CommandStruct();
maxSpeed = 10;
while true
% Read joystick axes
[axes, buttons, povs] = read(joy);
deadZone = abs(axes) < 0.1;
axes(deadZone) = 0;
% Map joystick inputs to motions
normVel = ...
[1 1 1 1] * -axes(5) + ... % forward (spin all same way)
[1 1 -1 -1] * axes(4) + ... % rotation (spin sides opposite ways)
[1 -1 -1 1] * axes(1); % sideways strafe
% Limit the summed velocities to the maximum
maxVel = max(abs(normVel));
if maxVel > 1
normVel = normVel / maxVel;
end
% Account for the physical mounting of the wheels on the
% robot, i.e., convert to local frames and send out
mounting = [-1 1 1 -1];
cmd.velocity = normVel .* mounting * maxSpeed;
wheels.set(cmd);
% Keep network from overloading by adding a small pause to
% prevent busy-spinning. This is only necessary because this
% demo does not rely on any sensor feedback.
pause(0.001);
end
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment