function advServoInit()
%ADVSERVOINIT Initialize the Advanced Servo controller
%   advServoInit() loads the loadphidget21 library, creates a global
%   handle to the Advanced Servo controller, and attempts to connect to 
%   an attached controller.
%
%   If a connection to an attached controller is successful then a global
%   variable ADVSERVO is created that is a handle to the attached
%   controller.

global ADVSERVO;

loadphidget21;

% is there alread a connected controller?
if ~isempty(ADVSERVO)
    if ~isNull(ADVSERVO)
        deviceStatus = libpointer('int32Ptr', -1);
        calllib('phidget21', 'CPhidget_getDeviceStatus', ADVSERVO, deviceStatus);
        if deviceStatus.Value == 1
            disp('Advanced Servo device already connected')
            return
        else
            cleanUp(ADVSERVO);
            ADVSERVO = [];
        end
    end
end


handle = libpointer('int32Ptr');
calllib('phidget21', 'CPhidgetAdvancedServo_create', handle);
calllib('phidget21', 'CPhidget_open', handle, -1);

if calllib('phidget21', 'CPhidget_waitForAttachment', handle, 2500) == 0    
    ADVSERVO = handle;
    disp('Connected to an Advanced Servo device')
else
    disp('Could not open Advanced Servo')
    cleanUp(handle);
end

end


function cleanUp(handle)
    calllib('phidget21', 'CPhidget_close', handle);
    calllib('phidget21', 'CPhidget_delete', handle);
end
