%Spaceball 4000FLX data
%Andrew Seltzman
%www.rtftechnologies.org

clc;
clear all;
%close all;
delete(instrfind);


%set up serial port
s = serial('COM4','Terminator','CR');
set(s, 'FlowControl', 'none');
set(s, 'DataBits', 8);
set(s, 'Parity', 'none');
set(s, 'StopBit', 1);
set(s, 'BaudRate', 9600);
%set(s, 'BaudRate', 38400);
set(s, 'TimeOut',0.8);
s.DataTerminalReady='off';   %power/range button pulled low
% s.BytesAvailableFcnCount = 2;
% s.BytesAvailableFcnMode = 'byte';
% s.BytesAvailableFcn = @instrcallback;
fopen(s)

pause(0.5);
fprintf(s,'@');     %tell spaceball to reset
fscanf(s)
pause(0.1);
fprintf(s,'B');     %tell spaceball to beep
pause(0.1);
fscanf(s)
fprintf(s,'M');     %tell spaceball to get movement data

% data=[];
% time=[];

%---------------------
maxpoints=10;
%figure(1)
%xlim([0,maxpoints])

%tic

for(n=[1:maxpoints])
%fprintf(s,query); 
pause(0.1);
get_char=13;

% if s.BytesAvailable<3, continue,end
    
    first=fscanf(s,'%c',1);
    num_first=double(first);
    if double(first)==46||double(first)==75,
        get_char=3;
    elseif double(first)==68,
        get_char=13;
    end
    
%     response=[first,fscanf(s,'%13c',get_char)] %get character string
%     num_response=double(response);  %convert to double array

if num_first==46||num_first==75,    %if digital input
    response=[first,fscanf(s)] %get character string
    num_response=double(response);  %convert to double array
    disp(['dig= ',num2str(length(num_response))])
end


if num_first==68,                         %if movement data
    response=[first,fscanf(s,'%13c',get_char)] %get character string
    num_response=double(response);  %convert to double array
    
%     fx=num_response(3)*256+num_response(4)
%     fy=num_response(5)*256+num_response(6)
%     fy=num_response(7)*256+num_response(8)
%     tx=num_response(9)*256+num_response(10)
%     ty=num_response(11)*256+num_response(12)
%     tz=num_response(13)*256+num_response(14)
    
    disp(['Movement= ',num2str(length(num_response))])
end


%data=[data;response];
%time=[time,toc];
%plot(data,'Linewidth',2)
%xlim([0,maxpoints])
%drawnow

end
%legend({'Fx','Fy','Fz','Tx','Ty','Tz'})

delete(instrfind);