EV3.m

classdef EV3 < MaskedHandle
    % High-level class to work with physical bricks.
    %
    % This is the 'central' class (from user's view) when working with this toolbox. It
    % delivers a convenient interface for creating a connection to the brick and sending
    % commands to it. An EV3-object creates 4 Motor- and 4 Sensor-objects, one for each port.
    %
    %
    % Notes:
    %     * Creating multiple EV3 objects and connecting them to different physical bricks has not
    %       been thoroughly tested yet, but seems to work on a first glance.
    %     * When an input argument of a method is marked as optional, the argument needs to be
    %       'announced' by a preceding 2nd argument, which is a string containing the name of the argument.
    %       For example, Motor.setProperties may be given a power-parameter. The syntax would be as
    %       follows: *brickObject.motorA.setProperties('power', 50);*
    %
    %
    %
    % Attributes:
    %     motorA (Motor): Motor-object interfacing port A. See also :class:`Motor`.
    %     motorB (Motor): Motor-object interfacing port B. See also :class:`Motor`.
    %     motorC (Motor): Motor-object interfacing port C. See also :class:`Motor`.
    %     motorD (Motor): Motor-object interfacing port D. See also :class:`Motor`.
    %     sensor1 (Sensor): Motor-object interfacing port 1. See also :class:`Sensor`.
    %     sensor2 (Sensor): Motor-object interfacing port 2. See also :class:`Sensor`.
    %     sensor3 (Sensor): Motor-object interfacing port 3. See also :class:`Sensor`.
    %     sensor4 (Sensor): Motor-object interfacing port 4. See also :class:`Sensor`.
    %     debug (numeric in {0,1,2}): Debug mode. *[WRITABLE]*
    %
    %         - 0: Debug turned off
    %         - 1: Debug turned on for EV3-object -> enables feedback in the console about what firmware-commands have been called when using a method
    %         - 2: Low-level-Debug turned on -> each packet sent and received is printed to the console
    %
    %     batteryMode (string in {'Percentage', 'Voltage'}): Mode for reading battery charge. See also :attr:`batteryValue`. *[WRITABLE]*
    %     batteryValue (numeric): Current battery charge. Depending on batteryMode, the reading is either in percentage or voltage. See also :attr:`batteryMode`. *[READ-ONLY]*
    %     isConnected (bool): True if virtual brick-object is connected to physical one. *[READ-ONLY]*
    %
    % :: 
    %
    %     Example:
    %         # This example expects a motor at port A and a (random) sensor at port 1
    %          brick = EV3();
    %          brick.connect('usb');
    %          motorA = brick.motorA;
    %          motorA.setProperties('power', 50, 'limitValue', 720);
    %          motorA.start();
    %          motorA.waitFor();
    %          disp(brick.sensor1.value);
    %          brick.beep();
    %          delete brick;
    %

    properties
        % batteryMode (string in {'Percentage', 'Voltage'}): Mode for reading battery charge. [WRITABLE]
        % See also BATTERYVALUE
        batteryMode;

        % debug (numeric in {0,1,2}): Debug mode. [WRITABLE]
        %     - 0: Debug turned off
        %     - 1: (High-level-) Debug turned on for EV3-object - enables feedback in the
        %          console about what firmware-commands have been called when using a method
        %     - 2: Low-level-Debug turned on - each packet sent and received is printed to the
        %          console
        debug;
    end

    properties (Dependent)  % Parameters to be read directly from physical brick
        % batteryValue (numeric): Current battery charge. Depending on batteryMode, the reading
        %     is either in percentage or voltage. [READ-ONLY]
        % See also BATTERYMODE
        batteryValue;
    end

    properties (SetAccess = private)  % Read-only properties that are set internally
        % isConnected (bool): True if virtual brick-object is connected to physical one. [READ-ONLY]
        isConnected = false;

        % motorA (Motor): Motor-object interfacing port A.
        % See also MOTOR
        motorA;
        % motorB (Motor): Motor-object interfacing port B.
        % See also MOTOR
        motorB;
        % motorC (Motor): Motor-object interfacing port C.
        % See also MOTOR
        motorC;
        % motorD (Motor): Motor-object interfacing port D.
        % See also MOTOR
        motorD;

        % sensor1 (Sensor): Sensor-object interfacing port 1.
        % See also SENSOR
        sensor1;
        % sensor2 (Sensor): Sensor-object interfacing port 2.
        % See also SENSOR
        sensor2;
        % sensor3 (Sensor): Sensor-object interfacing port 3.
        % See also SENSOR
        sensor3;
        % sensor4 (Sensor): Sensor-object interfacing port 4.
        % See also SENSOR
        sensor4;
    end

    properties (Access = private)
        % commInterface (CommunicationInterface): Interface to communication layer
        %     All commands sent to the Brick are created and written through this object. Each
        %     Motor- and Sensor-object has a reference to it.
        commInterface = 0;
    end

    properties (Hidden, Access = private)  % Hidden properties for internal use only
        % init (bool): Indicates init-phase (i.e. constructor is running).
        init = true;
    end

    methods  % Standard methods
        %% Constructor
        function ev3 = EV3(varargin)
            % Sets properties of EV3-object and creates Motor- and Sensor-objects with default
            % parameters.
            %
            % Arguments:
            %     varargin: see setProperties(ev3, varargin).
            %
            % See also SETPROPERTIES / :meth:`setProperties(ev3, varargin)`

            ev3.setProperties(varargin{:});

            ev3.motorA = Motor('A', ev3, 'Debug', ev3.debug>0);
            ev3.motorB = Motor('B', ev3, 'Debug', ev3.debug>0);
            ev3.motorC = Motor('C', ev3, 'Debug', ev3.debug>0);
            ev3.motorD = Motor('D', ev3, 'Debug', ev3.debug>0);


            ev3.sensor1 = Sensor('1', ev3, 'Debug', ev3.debug>0);
            ev3.sensor2 = Sensor('2', ev3, 'Debug', ev3.debug>0);
            ev3.sensor3 = Sensor('3', ev3, 'Debug', ev3.debug>0);
            ev3.sensor4 = Sensor('4', ev3, 'Debug', ev3.debug>0);

            ev3.init = false;
        end

        function delete(ev3)
            % Disconnects from physical brick and deletes this instance.

            if ev3.isConnected
                ev3.disconnect();
            end
        end

        %% Connection
        function connect(ev3, varargin)
            % Connects EV3-object and its Motors and Sensors to physical brick.
            %
            % Arguments:
            %     connectionType (string in {'bt', 'usb'}): Connection type
            %     serPort (string in {'/dev/rfcomm1', '/dev/rfcomm2', ...}): Path to serial port
            %         (necessary if connectionType is 'bt'). *[OPTIONAL]*
            %     beep (bool): If true, EV3 beeps if connection has been established. *[OPTIONAL]*
            %
            %
            % ::
            %
            %     Example:
            %          % Setup bluetooth connection via com-port 0 
            %          brick = EV3();
            %          brick.connect('bt', 'serPort', '/dev/rfcomm0');
            %          % Setup usb connection, beep when connection has been established
            %          brick = EV3();
            %          brick.connect('usb', 'beep', 'on', );
            %
            % See also ISCONNECTED / :attr:`isConnected`

            if ev3.isConnected
                if isCommInterfaceValid(ev3.commInterface)
                    % Warning lead to confusion of students. Since brick connection is still working
		            % even if calling connect() multiple times, it is disabled. 
		            % warning('EV3::connect: Already connected. Resetting connection now...');
                    ev3.disconnect();
                else
                    warning(['EV3::connect: EV3.isConnected is set to ''True'', but ',...
                             'comm handle is invalid. Deleting invalid handle and ' ,...
                             'resetting EV3.isConnected now...']);

                    ev3.commInterface = 0;
                    ev3.isConnected = false;
                end
            end

            if nargin < 2
                 error('EV3::connect: Wrong number of input arguments.');
            end

            idxes = strcmpi('beep', varargin);
            idx = find([0, idxes(1:end-1)]);
            if ~isempty(idx)
                beep = varargin{idx}; %#ok<FNDSB>
                if ~isBool(beep)
                    error('EV3::connect: Argument after ''beep'' has to be a bool.');
                end
            else
                beep = false;
            end

            % Try to connect
            try
                % Connect to physical brick
                % -> Creating communication-handle implicitly establishes connection
                ev3.commInterface = CommunicationInterface(varargin{:}, 'debug', ev3.debug>=2);
                ev3.isConnected = true;

                if beep
                    ev3.beep();
                end
                
            catch ME
                % Something went wrong...
                ev3.isConnected = false;
                if isCommInterfaceValid(ev3.commInterface) && ev3.commInterface ~= 0
                    ev3.commInterface.delete();
                    ev3.commInterface = 0;
                end

                rethrow(ME);
            end
        end

        function disconnect(ev3)
            % Disconnects EV3-object and its Motors and Sensors from physical brick.
            %
            % Notes:
            %     * Gets called automatically when EV3-object is destroyed.
            %
            %
            % ::
            %
            %     Example:
            %          brick = EV3();
            %          brick.connect('bt', 'serPort', '/dev/rfcomm0');
            %          % do stuff 
            %          brick.disconnect();
            
            % Resetting needs a working connection in order to send reset-commands
            % to the Brick. If the connection has been aborted (e.g. by pulling the
            % USB-cord), the reset-methods would fail -> catch this error and for
            % now do nothing.
            try
                ev3.resetPhysicalBrick();
            catch ME
                % For now: ignore
            end

            % Delete handle to comm-interface
            if isCommInterfaceValid(ev3.commInterface) && ev3.commInterface ~= 0
                ev3.commInterface.delete();
            end
            ev3.commInterface = 0;

            ev3.isConnected = false;
        end

        %% Device functions
%         function stopAllMotors(ev3)
%             % Sends a stop-command to all motor-ports.
%             ev3.handleCommand(@outputStop, 0, MotorBitfield.MotorA ...
%                                              +MotorBitfield.MotorB ...
%                                              +MotorBitfield.MotorC ...
%                                              +MotorBitfield.MotorD, 0);
%         end
        
        function resetAllDeviceValues(ev3)
            % Resets values of all sensors on all ports (including tacho counter on motors)
            ev3.handleCommand(@inputDeviceClrAll, 0);
        end

        %% Sound functions
        function beep(ev3)
            % Plays a 'beep'-tone on brick.
            %
            % Notes:
            %     * This equals playTone(10, 1000, 100).
            %
            %
            % ::
            %
            %     Example:
            %          brick = EV3();
            %          brick.connect('bt', 'serPort', '/dev/rfcomm0');
            %          brick.beep();
            %
            ev3.handleCommand(@soundPlayTone, 10, 1000, 100);
        end

        function playTone(ev3, volume, frequency, duration)
            % Plays tone on brick.
            %
            % Arguments:
            %     volume (numeric in [0, 100]): in percent
            %     frequency (numeric in [250, 10000]): in Hertz
            %     duration (numeric > 0): in milliseconds
            %
            %
            % ::
            %
            %    Example:
            %         brick = EV3();
            %         brick.connect('bt', 'serPort', '/dev/rfcomm0');
            %         brick.playTone(40, 5000, 1000);  % Plays tone with 40% volume and 5000Hz for 1 second.
            %
            ev3.handleCommand(@soundPlayTone, volume, frequency, duration);
        end

        function stopTone(ev3)
            % Stops tone currently played.
            %
            %
            % ::
            %
            %     Example:
            %          brick = EV3();
            %          brick.connect('bt', 'serPort', '/dev/rfcomm0');
            %          brick.playTone(10,100,100000000);
            %          brick.stopTone();  % Stops tone immediately.
            %
            ev3.handleCommand(@soundStopTone);
        end

        function status = tonePlayed(ev3)
            % Tests if tone is currently played.
            %
            % Returns:
            %     status (bool): True if a tone is being played
            %
            %
            % ::
            %
            %     Example:
            %          brick = EV3();
            %          brick.connect('bt', 'serPort', '/dev/rfcomm0');
            %          brick.playTone(10, 100, 1000);
            %          pause(0.5); 
            %          % Small pause necessary since tone not startong immediately 
            %          brick.tonePlayed(); % -> Outputs 1 to console. 
            %
            status = ev3.handleCommand(@soundTest);
        end

        %% Setter
        function set.commInterface(ev3, comm)
            if ~isCommInterfaceValid(comm)
                error('EV3::set.commInterface: Handle to Brick-object not valid.');
            end

            ev3.commInterface = comm;
        end

        function set.batteryMode(ev3, batteryMode)
            validModes = {'Voltage', 'Percentage'};
            if ~ischar(batteryMode) || ~ismember(batteryMode, validModes)
                error('EV3::set.batteryMode: Given parameter is not a valid battery mode.');
            end

            ev3.batteryMode = batteryMode;
        end

        function set.debug(ev3, debug)
            if ~isBool(debug) && debug ~= 2
                error('EV3::set.debug: Given parameter is not a bool.');
            end

            ev3.debug = str2bool(debug);

            if ev3.isConnected
                ev3.commInterface.debug = (ev3.debug >= 2);
            end

            ev3.motorA.debug = (ev3.debug > 0);
            ev3.motorB.debug = (ev3.debug > 0);
            ev3.motorC.debug = (ev3.debug > 0);
            ev3.motorD.debug = (ev3.debug > 0);

            ev3.sensor1.debug = (ev3.debug > 0);
            ev3.sensor2.debug = (ev3.debug > 0);
            ev3.sensor3.debug = (ev3.debug > 0);
            ev3.sensor4.debug = (ev3.debug > 0);
        end

        function setProperties(ev3, varargin)
            % Set multiple EV3 properties at once using MATLAB's inputParser.
            %
            % Arguments:
            %     debug (numeric in {0,1,2}): see EV3.debug *[OPTIONAL]*
            %     batteryMode (string in {'Voltage'/'Percentage'}): see EV3.batteryMode *[OPTIONAL]*
            %
            %
            % ::
            %
            %     Example:
            %          brick = EV3();
            %          brick.connect('bt', 'serPort', '/dev/rfcomm0');
            %          brick.setProperties('debug', 'on', 'batteryMode', 'Voltage');
            %          % Instead of: b.debug = 'on'; b.batteryMode = 'Voltage';
            %
            % See also EV3.DEBUG, EV3.BATTERYMODE / :attr:`debug`, :attr:`batteryMode`

            p = inputParser();

            % Set default values
            if ev3.init
                defaultDebug = false;
                defaultBatteryMode = 'Percentage';
            else
                defaultDebug = ev3.debug;
                defaultBatteryMode = ev3.batteryMode;
            end

            % Add parameter
            p.addOptional('debug', defaultDebug);
            p.addOptional('batteryMode', defaultBatteryMode);

            % Parse...
            p.parse(varargin{:});

            % Set properties
            ev3.batteryMode = p.Results.batteryMode;
            ev3.debug = p.Results.debug;
        end

        %% Getter
        function bat = get.batteryValue(ev3)
            if ~ev3.isConnected
                warning(ID('noConnection'), 'EV3-Object not connected to physical EV3.');

                bat = 0;
                return;
            end

            bat = ev3.getBattery();
        end

        function display(ev3)
            % Displays EV3-properties and its devices.

            displayProperties(ev3);

            fprintf('\n\tDevices\n');
            props = properties(ev3);

            warning('off', 'all');  % Turn off warnings while reading values
            for i = 1:length(props)
                p = props{i};
                member = ev3.(p);
                
                if strcmp(class(member),'Sensor') || strcmp(class(member), 'Motor')
                    fprintf('\t%15s [Type: %s]\n', p, char(member.type));
                end
            end
            warning('on', 'all');
        end
    end

    methods (Access = private)  % Private brick functions that are wrapped by dependent params
        function varargout = handleCommand(ev3, command, varargin)
            % Execute a CommunicationInterface-method given as a handle
            %
            % As those methods have different, fixed numbers of output arguments, this quantity
            % has to be retrieved first.
            
            if ~ev3.isConnected
                msg = ['Brick-Object not connected physical brick. ',...
                       'You have to call connect(...) first!'];
                id = [ID(), ':', 'NotConnected'];
                
                throw(MException(id, msg));
            end            
            
            if ev3.debug
                fprintf('(DEBUG) Sending %s\n', func2str(command));
            end

            % Note: Arrg. MATLAB does not support nargout for class methods directly, so I have to
            % do this ugly workaround using strings. See
            % https://de.mathworks.com/matlabcentral/answers/96617-how-can-i-use-nargin-nargout-to-determine-the-number-of-input-output-arguments-of-an-object-method
            nOut = nargout(strcat('CommunicationInterface>CommunicationInterface.', func2str(command)));
            [varargout{1:nOut}] = ev3.dispatch(command, nOut, varargin{:});
        end
        
        function bat = getBattery(ev3)
            % Retrieve batteryValue from brick in current mode. (Wrapped by get.batteryValue)
            
            if strcmpi(ev3.batteryMode, 'Percentage')
                bat = ev3.handleCommand(@uiReadLbatt);
            else
                bat = ev3.handleCommand(@uiReadVbatt);
            end
        end

        function resetPhysicalBrick(ev3)
            % Resets Motors and Sensors.
            %
            % Notes:
            %     * Gets called automatically by EV3.disconnect.
            %
            % See also MOTOR.RESETPHYSICALMOTOR, SENSOR.RESETPHYSICALSENSOR

            sensors = {'sensor1', 'sensor2', 'sensor3', 'sensor4'};
            motors = {'motorA', 'motorB', 'motorC', 'motorD'};

            for i = 1:4
                motor = motors{i};
                sensor = sensors{i};
                ev3.(motor).resetPhysicalMotor();
                ev3.(sensor).resetPhysicalSensor();
            end
        end
    end
    
    methods (Access = {?Sensor, ?Motor})
        function varargout = dispatch(ev3, command, noOutputArguments, varargin)
            try
                [varargout{1:noOutputArguments}] = command(ev3.commInterface, varargin{:});
            catch ME
                if ~isempty(strfind(ME.identifier, 'CommError'))
                    warning('Lost connection to the Brick!');
                    %ev3.disconnect();
                else
                    warning('Something went wrong. Try to reset the connection.');
                end

                throw(ME);
            end
        end
    end
end