% Read and plot radar data from arduino.
% Daniel Sjoberg, 2019-01-27

% Some words on handling the USB-port where the arduino is connected. The
% first argument to 'serial()' is the name of the port, which depends on
% your computer.
%
% Windows: you find the arduino port by going to the Device Manager,
% typically ends up at 'COM5' or similar. 
%
% Linux: you may need to create a symbolic link like /dev/ttyS999 to
% /dev/ttyACM0 in order for matlab to read the USB port. For some reason,
% matlab reads only very specific tty:s under linux.
%
% When terminating this script by Ctrl-C, the port opened by 'fopen(ser)'
% is left open. You need to close this by issuing 'fclose(ser)', or
% 'fclose(instrfindall)' to close all ports. You can remove previously 
% defined ports by 'delete(instrfindall)'. If you end up with a mess,
% exit matlab and start again.

ser = serial('/dev/ttyS999', 'BaudRate', 9600);
fopen(ser);
h = figure();
angles = [0];         % Data structure to store angles
ranges = [0];         % Data structure to store ranges
x = 0;                % Data used for plotting
y = 0;                % Data used for plotting
p = plot(x, y, 'o'); hold on;
p.XDataSource = 'x';  % We use DataSources in order to update the plot
p.YDataSource = 'y';
lx = [0, 0];          % Data for plotting the dashed red sweep line
ly = [0, 200];
l = plot(lx, ly, 'r--');
l.XDataSource = 'lx';
l.YDataSource = 'ly';

% Set viewing parameters
xlim([-145, 145]);
ylim([0, 210]);
axis equal;

% Make a polar grid
dphi = 10;           % Spacing between phi grid lines, in degrees
phi_start = -45;     % Start phi grid
phi_end = 45;        % End phi grid
phi_grid = [phi_start:dphi:phi_end];
dr = 20;             % Spacing between radial grid lines, in cm
r_start = 20;        % Start r grid
r_end = 200;         % End r grid
r_grid = [r_start:dr:r_end];
phi_vec = linspace(-45, 45, 100);
for nphi=1:length(phi_grid)
    plot([0, r_end*sin(phi_grid(nphi)*pi/180)], [0, r_end*cos(phi_grid(nphi)*pi/180)], 'k:');
end
for nr=1:length(r_grid)
    plot(r_grid(nr)*sin(phi_vec*pi/180), r_grid(nr)*cos(phi_vec*pi/180), 'k:');
end


while 1
    str = fscanf(ser);  % Read data in string format from serial port
    disp(str(1:end-1)); % Display data on command line
    [data, count, errmsg] = sscanf(str, '%f,%f');  % Parse string into values
    if count == 2 && isempty(errmsg)  % Update the figure only if data were correctly read
        angle = data(1);
        range = data(2);
        idx = find(angles == angle);  % Test if angle already stored
        if length(idx) > 0            % If this angle already exists, simply update range
            ranges(idx) = range;  
        else                          % If new angle, add data to existing
            ranges = [ranges, range];
            angles = [angles, angle];
        end
        % Update data in figure and replot
        x = ranges.*sin(angles*pi/180);
        y = ranges.*cos(angles*pi/180);
        lx = [0, 200*sin(angle*pi/180)];
        ly = [0, 200*cos(angle*pi/180)];
        refreshdata(p);
        refreshdata(l);
        drawnow limitrate;  % The 'limitrate' option makes figure update more nicely
    end
end
fclose(ser);
