Skip to content
Snippets Groups Projects
Commit 11b4f291 authored by Tim Stadtmann's avatar Tim Stadtmann
Browse files

Wrap Motor-flags in own class 'MotorState'

parent c17347a1
Branches
Tags
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/Motor.m
+ 105
− 95
View file @ 11b4f291
......@@ -95,17 +95,7 @@ classdef Motor < MaskedHandle & dynamicprops
brakeMode_;
%% Flags
connectedToBrick = false; % Connection to physical Brick?
init = true; % Indicates 'init-phase' (True as long as constructor is running)
startedNotBusy = false; % Set to true if motor started w/o tacholimit and unsynced
sendPowerOnSet = false; % If true, OUTPUT_POWER is sent when setting power
% Bitfield representing which opCodes should be sent on Motor.start()
% The corresponding opCodes for each bit are (in Big Endian):
% * 1st Bit: OUTPUT_POWER (sets power on physical Brick)
% * 2nd Bit: OUTPUT_STOP (stops Brick; workaround for a bug, see motor.start, Note 2)
sendOnStart = 0;
state = MotorState();
end
properties (Hidden, Dependent, Access = 'private') % Hidden, dependent properties for internal use only
......@@ -128,7 +118,7 @@ classdef Motor < MaskedHandle & dynamicprops
%
motor.setProperties(varargin{:});
motor.init = false;
motor.state.init = false;
end
%% Brick functions
......@@ -153,7 +143,7 @@ classdef Motor < MaskedHandle & dynamicprops
%
% Check connection and if motor is already running
if ~motor.connectedToBrick
if ~motor.state.connectedToBrick
error('Motor::start: Motor-Object not connected to comm handle.');
elseif ~motor.physicalMotorConnected
error('Motor::start: No physical motor connected to Port %s',...
......@@ -166,7 +156,7 @@ classdef Motor < MaskedHandle & dynamicprops
% using a tacholimit), the sync cache has to be deleted (in general, syncedStop
% would do so)
if motor.isSynced
delete(motor.findprop('syncCache'));
delete(motor.findprop('slave'));
motor.internalReset(); % Better safe than sorry
end
......@@ -178,20 +168,20 @@ classdef Motor < MaskedHandle & dynamicprops
% Call appropriate function in commInterface depending on limitValue and limitMode
if motor.limitValue==0
if motor.sendOnStart > 0
if motor.state.sendOnStart > 0
% If stop-flag is set: call stop() and reset flag
if bitget(motor.sendOnStart, SendOnStart.Stop)
if bitget(motor.state.sendOnStart, SendOnStart.Stop)
motor.stop();
motor.sendOnStart = bitset(motor.sendOnStart, SendOnStart.Stop, 0);
motor.state.sendOnStart = bitset(motor.state.sendOnStart, SendOnStart.Stop, 0);
end
% If power-flag is set: call setPower() and reset flag if successful
if bitget(motor.sendOnStart, SendOnStart.Power)
if bitget(motor.state.sendOnStart, SendOnStart.Power)
success = motor.setPower(motor.power);
if ~success
motor.sendOnStart = bitset(motor.sendOnStart, SendOnStart.Power, 1);
motor.state.sendOnStart = bitset(motor.state.sendOnStart, SendOnStart.Power, 1);
else
motor.sendOnStart = bitset(motor.sendOnStart, SendOnStart.Power, 0);
motor.state.sendOnStart = bitset(motor.state.sendOnStart, SendOnStart.Power, 0);
end
end
......@@ -202,7 +192,7 @@ classdef Motor < MaskedHandle & dynamicprops
if motor.debug
fprintf('(DEBUG) Motor::start: Called outputStart on Port %s\n', port2str('Motor', motor.port));
end
motor.startedNotBusy = true;
motor.state.startedNotBusy = true;
else
limit = motor.limitValue - (motor.smoothStart + motor.smoothStop);
if limit < 0
......@@ -257,7 +247,7 @@ classdef Motor < MaskedHandle & dynamicprops
function stop(motor)
%stop Stops the motor
if ~motor.connectedToBrick
if ~motor.state.connectedToBrick
error('Motor::stop: Motor-Object not connected to comm handle.');
elseif ~motor.physicalMotorConnected
error('Motor::stop: No physical motor connected to Port %s',...
......@@ -273,7 +263,7 @@ classdef Motor < MaskedHandle & dynamicprops
fprintf('(DEBUG) Motor::stop: Called outputStop on Port %s\n', port2str('Motor', motor.port));
end
motor.startedNotBusy = false;
motor.state.startedNotBusy = false;
end
function syncedStart(motor, syncMotor, varargin)
......@@ -334,7 +324,7 @@ classdef Motor < MaskedHandle & dynamicprops
end
% Check connection and motor parameter
if ~motor.connectedToBrick || ~syncMotor.connectedToBrick
if ~motor.state.connectedToBrick || ~syncMotor.state.connectedToBrick
error('Motor::syncedStart: Motor-Object not connected to comm handle.');
elseif ~motor.physicalMotorConnected || ~syncMotor.physicalMotorConnected
error('Motor::syncedStart: No physical motor connected to Port %s or %s.',...
......@@ -356,32 +346,33 @@ classdef Motor < MaskedHandle & dynamicprops
motor.internalReset();
end
if motor.sendOnStart > 0
if motor.state.sendOnStart > 0
% If stop-flag is set: call stop() and reset flag
if bitget(motor.sendOnStart, SendOnStart.Stop)
if bitget(motor.state.sendOnStart, SendOnStart.Stop)
motor.stop();
motor.sendOnStart = bitset(motor.sendOnStart, SendOnStart.Stop, 0);
motor.state.sendOnStart = bitset(motor.state.sendOnStart, SendOnStart.Stop, 0);
end
end
% Cache old values to make it possible to reset them on stopSynced
% Note: the existence of 'syncCache' is also used to determine whether motor is
% Note: the existence of 'slave' is also used to determine whether motor is
% running synchronized or not, see get.isSynced()
if motor.isSynced
meta = motor.findprop('syncCache');
else
meta = motor.addprop('syncCache');
meta = motor.findprop('slave');
if isempty(meta)
meta = motor.addprop('slave');
meta.Hidden = true;
meta.Access = 'private';
end
motor.syncCache.master_oldSendPowerOnSet = motor.sendPowerOnSet;
motor.syncCache.slave_oldSendPowerOnSet = syncMotor.sendPowerOnSet;
motor.syncCache.slave = syncMotor;
meta.Hidden = true;
meta.Access = 'private';
motor.slave = syncMotor;
motor.saveState();
syncMotor.saveState();
% Disable immediate sending of new power values
motor.sendPowerOnSet = false;
syncMotor.sendPowerOnSet = false;
motor.state.sendPowerOnSet = false;
syncMotor.state.sendPowerOnSet = false;
% Keep 'slave'-motor synchronized
syncMotor.speedRegulation = false;
......@@ -420,30 +411,29 @@ classdef Motor < MaskedHandle & dynamicprops
end
% Retrieve synced motor from cache
syncMotor = motor.syncCache.slave;
syncMotor = motor.slave;
delete(motor.findprop('slave'));
if ~motor.connectedToBrick || ~syncMotor.connectedToBrick
if ~motor.state.connectedToBrick || ~syncMotor.state.connectedToBrick
error('Motor::syncedStop: Motor-Object not connected to comm handle.');
elseif ~motor.physicalMotorConnected || ~syncMotor.physicalMotorConnected
error('Motor::syncedStop: No physical motor connected to either Port %s or %s.',...
port2str('Motor', motor.port), port2str('Motor', syncMotor.port));
end
% Retrieve other values from cache and delete it
motor.sendPowerOnSet = motor.syncCache.master_oldSendPowerOnSet;
%syncMotor.sendPowerOnSet = motor.syncCache.slave_oldSendPowerOnSet;
syncMotor.sendPowerOnSet = motor.syncCache.master_oldSendPowerOnSet;
delete(motor.findprop('syncCache'));
% Reset state
motor.applyState();
syncMotor.applyState();
% Synced stopping
motor.commInterface.outputStop(0, motor.port+syncMotor.port, motor.brakeMode_);
% On next start, both motors have to send power-opcode again
if motor.sendPowerOnSet
motor.sendOnStart = bitset(motor.sendOnStart, SendOnStart.Power, 1);
if motor.state.sendPowerOnSet
motor.state.sendOnStart = bitset(motor.state.sendOnStart, SendOnStart.Power, 1);
end
if syncMotor.sendPowerOnSet
syncMotor.sendOnStart = bitset(syncMotor.sendOnStart, SendOnStart.Power, 1);
if syncMotor.state.sendPowerOnSet
syncMotor.state.sendOnStart = bitset(syncMotor.state.sendOnStart, SendOnStart.Power, 1);
end
......@@ -472,7 +462,7 @@ classdef Motor < MaskedHandle & dynamicprops
% returns 0 if it's not
%
if ~motor.connectedToBrick
if ~motor.state.connectedToBrick
error('Motor::waitFor: Motor-Object not connected to comm handle.');
end
......@@ -525,7 +515,7 @@ classdef Motor < MaskedHandle & dynamicprops
% See also MOTOR.RESETTACHOCOUNT
%
if ~motor.connectedToBrick
if ~motor.state.connectedToBrick
error(['Motor::internalReset: Motor-Object not connected to brick handle.',...
'You have to call motor.connect(brick) first!']);
elseif ~motor.physicalMotorConnected
......@@ -542,7 +532,7 @@ classdef Motor < MaskedHandle & dynamicprops
end
function resetTachoCount(motor)
if ~motor.connectedToBrick
if ~motor.state.connectedToBrick
error('Motor::resetTachoCount: Motor-Object not connected to comm handle.');
elseif ~motor.physicalMotorConnected
error('Motor::resetTachoCount: No physical motor connected to Port %s',...
......@@ -576,7 +566,7 @@ classdef Motor < MaskedHandle & dynamicprops
motor.releaseBrake();
end
motor.sendOnStart = SendOnStart.Power + SendOnStart.Stop;
motor.state.sendOnStart = SendOnStart.Power + SendOnStart.Stop;
end
%% Setter
......@@ -590,12 +580,12 @@ classdef Motor < MaskedHandle & dynamicprops
motor.power = power; % Set power parameter.
if motor.sendPowerOnSet
if motor.state.sendPowerOnSet
success = motor.setPower(power);
if ~success
motor.sendOnStart = bitset(motor.sendOnStart, SendOnStart.Power, 1);
motor.state.sendOnStart = bitset(motor.state.sendOnStart, SendOnStart.Power, 1);
else
motor.sendOnStart = bitset(motor.sendOnStart, SendOnStart.Power, 0);
motor.state.sendOnStart = bitset(motor.state.sendOnStart, SendOnStart.Power, 0);
end
end
end
......@@ -603,7 +593,7 @@ classdef Motor < MaskedHandle & dynamicprops
function set.speedRegulation(motor, speedRegulation)
if ~isBool(speedRegulation)
error('Motor::set.speedRegulation: Given parameter is not a bool.');
% elseif motor.connectedToBrick && motor.physicalMotorConnected
% elseif motor.state.connectedToBrick && motor.physicalMotorConnected
% pause(0.5);
% if motor.currentSpeed ~= 0
% error(['Motor::set.speedRegulation: Cannot change speed regulation while ', ...
......@@ -614,8 +604,8 @@ classdef Motor < MaskedHandle & dynamicprops
speedRegulation = str2bool(speedRegulation);
if ~isempty(motor.speedRegulation) && (speedRegulation ~= motor.speedRegulation)
if motor.sendPowerOnSet
motor.sendOnStart = bitset(motor.sendOnStart, SendOnStart.Power, 1);
if motor.state.sendPowerOnSet
motor.state.sendOnStart = bitset(motor.state.sendOnStart, SendOnStart.Power, 1);
end
end
......@@ -653,8 +643,8 @@ classdef Motor < MaskedHandle & dynamicprops
% If new brakeMode is 'Brake': reset internal tachocount once
% Note: if new brakeMode is 'Coast', internal tachocount is always reset
% right before starting, so it's not necessary here
if ~motor.init && strcmpi(brakeMode,'Brake') && ...
motor.connectedToBrick && motor.physicalMotorConnected
if ~motor.state.init && strcmpi(brakeMode,'Brake') && ...
motor.state.connectedToBrick && motor.physicalMotorConnected
motor.internalReset();
end
......@@ -681,14 +671,14 @@ classdef Motor < MaskedHandle & dynamicprops
end
if limitValue == 0
motor.sendOnStart = SendOnStart.Power;
motor.state.sendOnStart = SendOnStart.Power;
if ~isempty(motor.limitValue) && motor.limitValue > 0
motor.sendOnStart = motor.sendOnStart + SendOnStart.Stop;
motor.state.sendOnStart = motor.state.sendOnStart + SendOnStart.Stop;
end
motor.sendPowerOnSet = true;
motor.state.sendPowerOnSet = true;
else
motor.sendOnStart = 0;
motor.sendPowerOnSet = false;
motor.state.sendOnStart = 0;
motor.state.sendPowerOnSet = false;
end
motor.limitValue= limitValue;
......@@ -745,7 +735,7 @@ classdef Motor < MaskedHandle & dynamicprops
p.KeepUnmatched = 1;
% Set default values
if motor.init
if motor.state.init
defaultDebug = 0;
defaultSpeedReg = 0;
defaultBrakeMode = 'Coast';
......@@ -766,7 +756,7 @@ classdef Motor < MaskedHandle & dynamicprops
end
% Add parameter
if motor.init
if motor.state.init
p.addRequired('port');
end
p.addOptional('debug', defaultDebug);
......@@ -782,7 +772,7 @@ classdef Motor < MaskedHandle & dynamicprops
p.parse(varargin{:});
% Set properties
if motor.init
if motor.state.init
motor.port = p.Results.port;
end
motor.power = p.Results.power;
......@@ -793,10 +783,6 @@ classdef Motor < MaskedHandle & dynamicprops
motor.speedRegulation = p.Results.speedRegulation;
motor.smoothStart = p.Results.smoothStart;
motor.smoothStop = p.Results.smoothStop;
% if motor.limitValue == 0
% motor.sendPowerOnSet = true;
% end
end
%% Getter
......@@ -810,7 +796,7 @@ classdef Motor < MaskedHandle & dynamicprops
function cnt = get.tachoCount(motor)
cnt = 0;
if motor.connectedToBrick
if motor.state.connectedToBrick
cnt = motor.getTachoCount();
if isnan(cnt)
warning('Motor::get.tachoCount: Could not detect motor at port %s.', ...
......@@ -822,7 +808,7 @@ classdef Motor < MaskedHandle & dynamicprops
function cnt = get.internalTachoCount(motor)
cnt = 0;
if motor.connectedToBrick
if motor.state.connectedToBrick
cnt = motor.getInternalTachoCount();
if isnan(cnt)
warning('Motor::get.internalTachoCount: Could not detect motor at port %s.', ...
......@@ -834,7 +820,7 @@ classdef Motor < MaskedHandle & dynamicprops
function speed = get.currentSpeed(motor)
speed = 0;
if motor.connectedToBrick
if motor.state.connectedToBrick
speed = motor.getSpeed();
if isnan(speed)
warning('Motor::get.currentSpeed: Could not detect motor at port %s.', ...
......@@ -847,22 +833,22 @@ classdef Motor < MaskedHandle & dynamicprops
function running = get.isRunning(motor)
running = 0;
if motor.connectedToBrick
if motor.state.connectedToBrick
busyFlag = motor.getBusyFlag();
else
busyFlag = 0;
end
assert(~(motor.startedNotBusy && busyFlag));
running = motor.startedNotBusy || busyFlag;
assert(~(motor.state.startedNotBusy && busyFlag));
running = motor.state.startedNotBusy || busyFlag;
end
function synced = get.isSynced(motor)
synced = (length(findprop(motor, 'syncCache'))==1);
synced = (length(findprop(motor, 'slave'))==1);
end
function motorType = get.type(motor)
if motor.connectedToBrick
if motor.state.connectedToBrick
[motorType, ~] = motor.getTypeMode();
else
motorType = DeviceType.Unknown;
......@@ -891,7 +877,7 @@ classdef Motor < MaskedHandle & dynamicprops
% with the new value instantly. However, this sometimes leads to unexpected behaviour.
% Therefore, if motor is running with a limit, setPower aborts with a warning.
%
if ~motor.connectedToBrick || ~motor.physicalMotorConnected
if ~motor.state.connectedToBrick || ~motor.physicalMotorConnected
% error('Motor::getTachoCount: Motor-Object not connected to comm handle.');
success = false;
return;
......@@ -919,7 +905,7 @@ classdef Motor < MaskedHandle & dynamicprops
end
function setMode(motor, mode) %% DEPRECATED
if ~motor.connectedToBrick
if ~motor.state.connectedToBrick
error('Motor::getTachoCount: Motor-Object not connected to comm handle.');
elseif ~motor.physicalMotorConnected
error('Motor::getTachoCount: No physical motor connected to Port %s',...
......@@ -936,7 +922,7 @@ classdef Motor < MaskedHandle & dynamicprops
end
function [type,mode] = getTypeMode(motor)
if ~motor.connectedToBrick
if ~motor.state.connectedToBrick
error('Motor::getTypeMode: Motor-Object not connected to comm handle.');
end
......@@ -951,7 +937,7 @@ classdef Motor < MaskedHandle & dynamicprops
end
function status = getStatus(motor)
if ~motor.connectedToBrick
if ~motor.state.connectedToBrick
error('Motor::getStatus: Motor-Object not connected to comm handle.');
end
......@@ -965,7 +951,7 @@ classdef Motor < MaskedHandle & dynamicprops
end
function cnt = getTachoCount(motor)
if ~motor.connectedToBrick
if ~motor.state.connectedToBrick
error('Motor::getTachoCount: Motor-Object not connected to comm handle.');
end
......@@ -983,7 +969,7 @@ classdef Motor < MaskedHandle & dynamicprops
end
function speed = getSpeed(motor)
if ~motor.connectedToBrick
if ~motor.state.connectedToBrick
error('Motor::getSpeed: Motor-Object not connected to comm handle.');
end
......@@ -1003,7 +989,7 @@ classdef Motor < MaskedHandle & dynamicprops
% * Busy is set to true if motor is running with tacholimit or synced
%
if ~motor.connectedToBrick
if ~motor.state.connectedToBrick
error('Motor::getBusyFlag: Motor-Object not connected to comm handle.');
end
......@@ -1015,7 +1001,7 @@ classdef Motor < MaskedHandle & dynamicprops
end
function applyBrake(motor)
if ~motor.connectedToBrick
if ~motor.state.connectedToBrick
error('Motor::applyBrake: Motor-Object not connected to comm handle.');
elseif ~motor.physicalMotorConnected
error('Motor::applyBrake: No physical motor connected to Port %s',...
......@@ -1039,7 +1025,7 @@ classdef Motor < MaskedHandle & dynamicprops
end
function releaseBrake(motor)
if ~motor.connectedToBrick
if ~motor.state.connectedToBrick
error('Motor::releaseBrake: Motor-Object not connected to comm handle.');
elseif ~motor.physicalMotorConnected
error('Motor::releaseBrake: No physical motor connected to Port %s',...
......@@ -1063,6 +1049,30 @@ classdef Motor < MaskedHandle & dynamicprops
end
end
methods (Access = 'private', Hidden = true)
function saveState(motor)
%saveState Saves current motor state in dynamic property
meta = motor.findprop('savedState');
if isempty(meta)
meta = motor.addprop('savedState');
meta.Hidden = true;
meta.Access = 'private';
end
motor.savedState = motor.state;
end
function applyState(motor)
%applyState Sets motor state to saved state and deletes the dynamic property in
%which the latter is stored
assert(length(motor.findprop('savedState')) ~= 0);
motor.state = motor.savedState;
delete(motor.findprop('savedState'))
end
end
methods (Access = {?EV3})
function connect(motor,commInterface)
......@@ -1082,13 +1092,13 @@ classdef Motor < MaskedHandle & dynamicprops
% % do stuff
%
if motor.connectedToBrick
if motor.state.connectedToBrick
if isCommInterfaceValid(motor.commInterface)
error('Motor::connect: Motor-Object already has a comm handle.');
else
warning(['Motor::connect: Motor.connectedToBrick is set to ''True'', but ',...
warning(['Motor::connect: Motor.state.connectedToBrick is set to ''True'', but ',...
'comm handle is invalid. Deleting invalid handle and ' ,...
'resetting Motor.connectedToBrick now...']);
'resetting Motor.state.connectedToBrick now...']);
motor.disconnect();
......@@ -1097,7 +1107,7 @@ classdef Motor < MaskedHandle & dynamicprops
end
motor.commInterface = commInterface;
motor.connectedToBrick = true;
motor.state.connectedToBrick = true;
if motor.debug
fprintf('(DEBUG) Motor-Object connected to comm handle.\n');
......@@ -1121,7 +1131,7 @@ classdef Motor < MaskedHandle & dynamicprops
%
motor.commInterface = 0; % Note: actual deleting is done in EV3::disconnect.
motor.connectedToBrick = false;
motor.state.connectedToBrick = false;
end
end
end
source/MotorState.m 0 → 100644
+ 41
− 0
View file @ 11b4f291
classdef MotorState
properties
connectedToBrick = false; % Connection to physical Brick?
init = true; % Indicates 'init-phase' (True as long as constructor is running)
startedNotBusy = false; % Set to true if motor started w/o tacholimit and unsynced
sendPowerOnSet = false; % If true, OUTPUT_POWER is sent when setting power
% Bitfield representing which opCodes should be sent on Motor.start()
% The corresponding opCodes for each bit are (in Big Endian):
% * 1st Bit: OUTPUT_POWER (sets power on physical Brick)
% * 2nd Bit: OUTPUT_STOP (stops Brick; workaround for a bug, see motor.start, Note 2)
sendOnStart = 0;
end
methods
function display(state)
fprintf('#### Motor State ####\n');
props = properties(state);
for i = 1:length(props)
p = props{i};
fprintf('%16s: %d\n', p, state.(p));
end
fprintf('#####################\n');
end
function isequal = eq(state1, state2)
props = properties(state1);
isequal = 1;
for i = 1:length(props)
p = props{i};
if state1.(p) ~= state2.(p)
isequal = 0;
break;
end
end
end
end
end
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment