113 lines
4.5 KiB
Matlab
113 lines
4.5 KiB
Matlab
classdef WheeledRobot
|
|
%WheeledRobot An abstract class supporting ododmetry for wheeled robots.
|
|
% Your implementing class should provide the method:
|
|
%
|
|
% extractOdometry(obj, timestamp, leftWheel, rightWheel) -->
|
|
% (timestampSeconds, leftWheelDegrees, rightWheelDegrees)
|
|
%
|
|
% Copyright (C) 2014 Simon D. Levy
|
|
%
|
|
% This code is free software: you can redistribute it and/or modify
|
|
% it under the terms of the GNU Lesser General Public License as
|
|
% published by the Free Software Foundation, either version 3 of the
|
|
% License, or (at your option) any later version.
|
|
%
|
|
% This code is distributed in the hope that it will be useful,
|
|
% but WITHOUT ANY WARRANTY without even the implied warranty of
|
|
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
% GNU General Public License for more details.
|
|
%
|
|
% You should have received a copy of the GNU Lesser General Public License
|
|
% along with this code. If not, see <http:#www.gnu.org/licenses/>.
|
|
|
|
properties (Access = 'private')
|
|
|
|
wheelRadiusMillimeters
|
|
halfAxleLengthMillimeters
|
|
|
|
timestampSecondsPrev
|
|
leftWheelDegreesPrev
|
|
rightWheelDegreesPrev
|
|
|
|
end
|
|
|
|
methods (Access = 'protected')
|
|
|
|
function s = str(obj)
|
|
% Returns a string representation of this WheeledRobot
|
|
s = sprintf('<Wheel radius=%f mm Half axle Length=%f mm>', ...
|
|
obj.wheelRadiusMillimeters, obj.halfAxleLengthMillimeters);
|
|
end
|
|
|
|
function robot = WheeledRobot(wheelRadiusMillimeters, halfAxleLengthMillimeters)
|
|
% Constructs a WheeledRobot object
|
|
% robot = WheeledRobot(wheelRadiusMillimeters, halfAxleLengthMillimeters)
|
|
robot.wheelRadiusMillimeters = wheelRadiusMillimeters;
|
|
robot.halfAxleLengthMillimeters = halfAxleLengthMillimeters;
|
|
|
|
end
|
|
|
|
function r = deg2rad(~, d)
|
|
% Converts degrees to radians
|
|
r = d * pi / 180;
|
|
end
|
|
|
|
end
|
|
|
|
methods (Abstract)
|
|
|
|
extractOdometry(obj, timestamp, leftWheel, rightWheel)
|
|
|
|
end
|
|
|
|
methods (Access = 'public')
|
|
|
|
function [velocities, obj] = computeVelocities(obj, timestamp, leftWheelOdometry, rightWheelOdometry)
|
|
% Computes forward and angular velocities based on odometry.
|
|
%
|
|
% Parameters:
|
|
|
|
% timestamp time stamp, in whatever units your robot uses
|
|
% leftWheelOdometry odometry for left wheel, in whatever form your robot uses
|
|
% rightWheelOdometry odometry for right wheel, in whatever form your robot uses
|
|
%
|
|
% Returns a tuple (dxyMillimeters, dthetaDegrees, dtSeconds)
|
|
|
|
% dxyMillimeters forward distance traveled, in millimeters
|
|
% dthetaDegrees change in angular position, in degrees
|
|
% dtSeconds elapsed time since previous odometry, in seconds
|
|
|
|
dxyMillimeters = 0;
|
|
dthetaDegrees = 0;
|
|
dtSeconds = 0;
|
|
|
|
[timestampSecondsCurr, leftWheelDegreesCurr, rightWheelDegreesCurr] = ...
|
|
obj.extractOdometry(timestamp, leftWheelOdometry, rightWheelOdometry);
|
|
|
|
if obj.timestampSecondsPrev
|
|
|
|
leftDiffDegrees = leftWheelDegreesCurr - obj.leftWheelDegreesPrev;
|
|
rightDiffDegrees = rightWheelDegreesCurr - obj.rightWheelDegreesPrev;
|
|
|
|
dxyMillimeters = obj.wheelRadiusMillimeters * ...
|
|
(obj.deg2rad(leftDiffDegrees) + obj.deg2rad(rightDiffDegrees));
|
|
|
|
dthetaDegrees = (obj.wheelRadiusMillimeters / obj.halfAxleLengthMillimeters) * ...
|
|
(rightDiffDegrees - leftDiffDegrees);
|
|
|
|
dtSeconds = timestampSecondsCurr - obj.timestampSecondsPrev;
|
|
end
|
|
|
|
% Store current odometry for next time
|
|
obj.timestampSecondsPrev = timestampSecondsCurr;
|
|
obj.leftWheelDegreesPrev = leftWheelDegreesCurr;
|
|
obj.rightWheelDegreesPrev = rightWheelDegreesCurr;
|
|
|
|
% Return linear velocity, angular velocity, time difference
|
|
velocities = [dxyMillimeters, dthetaDegrees, dtSeconds];
|
|
|
|
end
|
|
end
|
|
end
|
|
|