% -------------------------------------------------------------------------
%Author:
%Sebastian Friston, 
%University College London, 
%Dept. of Computer Science
%email: sebastian.friston.12@ucl.ac.uk
%
%Date:
%20/11/2013
%
%This file is provided as part of the supplementary materials for Measuring
%Latency in Virtual Environments.
% -------------------------------------------------------------------------

classdef ServoDriver
%SERVODRIVER Class to control the servo set via a serial link
%   Detailed explanation goes here
    
    properties
        SamplesPerSecond = 50;
    end

    properties(GetAccess = private)
        s; % the serial port object
    end

    methods
        function obj = ServoDriver(serialPort)
            % Create a new ServoDriver controller and connect to the board
            % on the serial port provided.
            obj.s = serial(serialPort);
            fopen(obj.s);
        end
        
        function Reset(obj)
           % Attempt to reopen the connection to the current serial port.
           fclose(obj.s);
           fopen(obj.s);
        end
        
        function y = CreateWave(obj, frequency, length, range)
           % Create a set of samples to generate servo motion. (Frequency (Hz), Length (s), Range of motion (between 0-1).
           x = 1:(obj.SamplesPerSecond * length);
           y = round(((sin((x * 2 * pi * frequency) / obj.SamplesPerSecond) * range) + 1) * 127);
        end
        
        function SendWave(obj, y)
           % Send a single or a set of samples to be played back at 50Hz
           fwrite(obj.s, [255 0], 'uint8');
           fwrite(obj.s, y, 'uint8');
        end
        
        function SetDelay(obj, delay)
           % Set the delay after which the second servo follows the first (in ms)
           fwrite(obj.s, [255 1], 'uint8');
           fwrite(obj.s, delay, 'uint8');
           fwrite(obj.s, [255 0], 'uint8');
        end
        
        function LoadDelay(obj, delay)
           % disable writing then set the delay
           fwrite(obj.s, [255 3], 'uint8');
           fwrite(obj.s, delay, 'uint8');
        end
        
        function LoadWave(obj, y)
           fwrite(obj.s, [255 2], 'uint8');
           fwrite(obj.s, y, 'uint8');
        end
        
        function Start(obj)
            fwrite(obj.s, [255 0], 'uint8');
        end
        
        function SendWaveAndDelay(obj, y, d)
           LoadDelay(obj,d);
           LoadWave(obj,y);
           Start(obj);
        end
        
    end

end