% Motor control for the portable skin stretch device
% ***For Shinsei motor and driver number: 55520

clear all
% ===========================================
% Initialize phidgets I/O board
if ~libisloaded('phidget21')
    loadlibrary('phidget21', 'phidget21Matlab.h');
end

% Attach Phidgets I/O board
ptr = libpointer('int32Ptr', 0);
calllib('phidget21', 'CPhidgetInterfaceKit_create', ptr);
handle = get(ptr, 'Value');
calllib('phidget21', 'CPhidget_open', handle, -1);

% Attach Phidgets encoder
ptr2 = libpointer('int32Ptr', 0);
calllib('phidget21', 'CPhidgetEncoder_create', ptr2);
handle2 = get(ptr2, 'Value');
calllib('phidget21', 'CPhidget_open', handle2, -1);

% ============================================
% Run control code
if ( (calllib('phidget21', 'CPhidget_waitForAttachment', handle, 2500)) == 0 && (calllib('phidget21', 'CPhidget_waitForAttachment', handle2, 2500) == 0))
    dataptr = libpointer('int32Ptr',0);
    speed_volts(1) = 0;
    direction_volts(1) = 0;

    Ts = 0.01; %sec
    time_len = 1; %sec
    num_points = time_len/Ts;
    t = zeros(1,num_points);
        
    dest_tolerance = 0.1; % deg
    kp = 0.5;

   
    step_amp = 20; %deg
    sin_amp = 20; %deg
    sin_freq = 0.5; %Hz
    
    tic;
    pause(0.025);
    timestamp = 0.025;
    % Start loop
    for i = 1:num_points
        while(timestamp < Ts*i)
            pause(Ts/2);
            timestamp = toc;
        end

        % Get current location
        calllib('phidget21', 'CPhidgetEncoder_getPosition', handle2, 0, dataptr);
        encoder(i) = get(dataptr, 'Value');
        pos(i) = double(encoder(i)) * (360/5000); %convert encoder counts to deg
        t(i) = timestamp;

        % Control calcs
        pos_desired(i) = step_amp;
%         pos_desired(i) = sin_amp * sin(2*pi*sin_freq*timestamp);
        diff = pos_desired(i) - pos(i);
        if abs(diff) < dest_tolerance
            speed_volts(i) = 0;
            direction_volts(i) = 0;
        else
            speed_volts(i) = abs(kp*diff);
            direction_volts(i) = sign(diff);
        end

        setMotorSpd(speed_volts(i),direction_volts(i),handle);
    end
else
    disp('Could not attach interfacekit')
end

% Clean up
setMotorSpd(0,0,handle);
calllib('phidget21', 'CPhidget_close', handle);
calllib('phidget21', 'CPhidget_delete', handle);
calllib('phidget21', 'CPhidget_close', handle2);
calllib('phidget21', 'CPhidget_delete', handle2);