%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% optical_sensor_serial()
% Test function for reading optical sensor data from ADNS-3080.
% Nicole Rodia (ncr6)
% William Westerick (wjw27)
%
% To use this code, send sensor data over the UART. 
% 4 bytes x-coord, 4 bytes y-coord, 4 bytes rotation
% Format: All values must be 32-bit integers sent in 4 consecutive bytes.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function optical_sensor_serial()
clc;
%needed to close the connection if the program is terminated
%fclose(instrfind); 
s = serial('COM1');     %create the serial object
%Set the tx/rx parameters
s.InputBufferSize = 50000;
s.BaudRate = 38400;
set(s,'FlowControl','none');
set(s,'Parity','none');
set(s,'Terminator','LF');
set(s,'ByteOrder','bigEndian');
fopen(s)                %open the connection
s.ReadAsyncMode = 'continuous';
%Initialize variables
NumDataTx = 0;
value = [0 0 0];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Starting...');
pause(1)
%Set up the plot windows
figure(1); clf(); hold on;
figure(2); clf(); hold on;

while(NumDataTx < 10000) 
while(s.BytesAvailable >= 12)
    data = fread(s, 3, 'int32');  %read the data from the buffer
    prevvalue = value;
    value = (data/1024/400)';

    NumDataTx = NumDataTx + 1;    %increment the data transfer counter
    if(length(data ~= 0))
        figure(1);
        %Plot the rotational motion
        plot([prevvalue(1) value(1)],[prevvalue(2) value(2)]);
        axis equal;
        figure(2);
        %Plot the rotational motion
        plot([0 (10)*cos(value(3)/3)],[0 (10)*sin(value(3)/3)]);
        axis equal;
    end 
end %if
pause(0.01);
end %while
    
pause(1)
fclose(s)    %close the connection
delete(s)
clear s
end %optical_sensor_serial