function plot_dual_ADXL362
% Function to read and plot data from two accelerometers connected
% to a USB port through an Arduino board
%
% You can edit this function to display the data any way you like....
% Check for recent MATLAB version (serial port management has changed in 2021a)
  newmat = false;
  [~,verdate] = version;
  verdate = strsplit(verdate);
  verdate = str2num(verdate{3});
  if (verdate>2020)
      newmat = true;
  end
  close all
  closeSerial(newmat)  % If the serial port is open, close it


% Open the serial port.   You may need to change the COM4 to 
% some other setting (COM3, COM5, etc) depending on where the
% USB device gets mapped on your laptop.
% On a Mac this will be something like /dev/cu/xxxxx instead
% The Arduino editor will show you the active ports for your
% computer.

  comport = 'COM3';  % Change this to match your com port 

  if (~exist('SerialFlag','var'))
       [dataport.s,accel_settings,flag] = setupSerial(comport,newmat);
  end
% Use the function below to double the delay time between data points
% (each time you call the function it will double the delay time)
 accel_settings = increase_delaytime(accel_settings,dataport,newmat);
% accel_settings = increase_delaytime(accel_settings,dataport,newmat);
% accel_settings = increase_delaytime(accel_settings,dataport,newmat);
%  accel_settings = increase_delaytime(accel_settings,dataport,newmat);
%   accel_settings = increase_delaytime(accel_settings,dataport,newmat);
%    accel_settings = increase_delaytime(accel_settings,dataport,newmat);
%     accel_settings = increase_delaytime(accel_settings,dataport,newmat);
%      accel_settings = increase_delaytime(accel_settings,dataport,newmat);
%       accel_settings = increase_delaytime(accel_settings,dataport,newmat);
%        accel_settings = increase_delaytime(accel_settings,dataport,newmat);
 %
% Use the function below to switch the accelerometer sensitivity
% to the next level (+/- 2g; 4g; 8g).   Enter 'a' or 'b' to select
% the accelerometer.
%accel_settings = toggle_ADXL362_sensitivity('a',accel_settings,dataport,newmat);
%
%  This function will repeatedly burst read the accelerometers
% and acquire 100 data points (ax,ay,az on both accelerometers),
% and then upload this data to Matlab and plot a graph of accel-v-time.
% Uploading the data will take about 20 seconds.
% The default spacing between datapoints is about 1 milliseconds - this
% is the best setting for measuring at 45Hz.   To measure at 10Hz you
% will need a delay of about 4 milliseconds. 
%
[timevals,accelvals] = scan_accels(accel_settings,dataport,newmat);

% This will print a comma separated value file with time (in milliseconds),
% and ax,ay,az (accel a) ax,ay,az (accel b) in m/s^2 as columns
% The data for the most recently plotted graph will be stored.
% You can open the file with excel, or read it into matlab with
% csvread (same as the quadcopter from the start of the semester)
% csvwrite('accel_data.csv',[timevals(:),accelvals(:,1:6)]);

% Serial port should have closed already but closed again in case
% there was a glitch...
closeSerial(newmat);
end
function [s,accel_settings,flag] = setupSerial(comPort,newversion)
%  Open the serial port and set default values for accelerometer
%  settings
%  accel_settings(1) = accelerometer a range setting (+/- 2g is default)
%  accel_settings(2) = accelerometer b range (+/- 2g is default)
%  accel_settings(3) = time between sample points in a scan (1ms is
%  default)
    accel_settings = [2,2,1];   
    flag = 1;
    if (newversion)
    %  Open the serial port and check the communication
        s = serialport(comPort,115200,'Timeout',3);
    %   Check the connection
        a = 'b';
        while (a ~='a')
            a=read(s,1,'char');
        end
        cmd = sprintf("a");
        writeline(s,cmd);
        mbox = msgbox('Press OK to begin measurement');
        uiwait(mbox);
        r = readline(s);
        sscanf(r,'%u');
    else
        s = serial(comPort);
        set(s,'DataBits',8);
        set(s,'StopBits',1);
        set(s,'BaudRate',115200);
        set(s,'Parity','none');
        fopen(s);
    %   Check the connection
        a = 'b';
        while (a ~='a')
            a=fread(s,1,'uchar');
        end
        fprintf(s,'%c','a');
        mbox = msgbox('Press OK to begin measurement');
        uiwait(mbox);
        fscanf(s,'%u');
    end
end
function closeSerial(newmat)
%   Close the serial port
    if (~newmat)
        if ~isempty(instrfind)
            fclose(instrfind);
            delete(instrfind);
        end
    end
    h = gcbo;  % Set window button value to zero
    set(h,'UserData',0);
end

function [timevals,accelvals] = scan_accels(accel_settings,dataport,newmat)
% This function repeatedly measures 100 acceleration and time
% values in a burst read and then plots them. This repeats
% until told to stop by the user
% accel_settings stores the current settings for the accelerometers
% 'dataport' is the serial port object created by setupSerial
% newmat set to true for Matlab version > 2020
%
%  Data will be displayed in a figure with a separate plot
%  for each accelerometer.  A textbox shows the rms amplitudes
%  of each acceleration component, and buttons enable the user
%  to toggle the delay time between accelerometer readings and the
%  sensitivity of each accelerometer.
%
%
% Create the plot
if(~exist('h', 'var') || ~ishandle(h))
    scrsz = get(0,'ScreenSize'); % Get the user's screen size
    h = figure('Position',[scrsz(3)/5 scrsz(4)/5 5*scrsz(3)/6 2*scrsz(4)/3],'Color',[1 1 1]);
    % Create axes and label them
    axes1 = axes('Parent',h,...
    'Position',[0.586228956228955 0.112320185614849 0.343905723905724 0.815]);
    box(axes1,'on');

    axes2 = axes('Parent',h,...
    'Position',[0.196498316498316 0.114640371229698 0.316969696969697 0.815]);
    box(axes2,'on');
    hold(axes2,'on');
    xlabel('Time (milliseconds)');
    ylabel('Acceleration (m/s^2)');
    title({'Accelerometer A'});
end
% Add buttons to figure window
if (~exist('button','var'))
    guidata(h,struct('SensitivityA',accel_settings(1),...
                     'SensitivityB',accel_settings(2),...
                     'DelayTime',accel_settings(3),...
                     'DataPort',dataport,...
                     'NewMatlab',newmat));
    % Stop button
    button = uicontrol('Style','pushbutton','String','Stop',...
        'pos',[5 0 50 25], 'parent',h,'CallBack',@stop_button_Callback,...
        'UserData',1);
    % Button to decrease delay time for accelerometer reads
    button2 = uicontrol('Style','pushbutton','String','Decrease Delay Time',...
        'pos',[5 30 130 25], 'parent',h,'CallBack',@decrease_delaytime_Callback);
    % Button to increase delay time for accelerometer reads
    button3 = uicontrol('Style','pushbutton','String','Increase Delay Time',...
        'pos',[5 60 130 25], 'parent',h,'CallBack',@increase_delaytime_Callback);
    % Button to toggle sensitivity of accelerometer B
    button4 = uicontrol('Style','pushbutton','String','Toggle B Sensitivity',...
        'pos',[5 90 130 25], 'parent',h,'CallBack',@B_sensitivity_Callback);
    % Button to toggle sensitivity of accelerometer A
    button5 = uicontrol('Style','pushbutton','String','Toggle A Sensitivity',...
        'pos',[5 120 130 25], 'parent',h,'CallBack',@A_sensitivity_Callback);
    
end
% Continue reading accelerations and plotting until stop button is pressed
while(get(button,'UserData'))
    data = guidata(h);
    accel_settings = [data.SensitivityA,data.SensitivityB,data.DelayTime];
    newmat = data.NewMatlab;
%   Read accelerometer data from Arduino board
    [timevals,accelvals] = read_accelerometer_history(accel_settings,dataport,newmat);
%   Plot the data. 
    hold(axes2,'off');
    hold(axes1,'off');
    plot1 = plot(timevals,accelvals(:,1:3),'Parent',axes2);
    plot2 = plot(timevals,accelvals(:,4:6),'Parent',axes1);
    hold(axes1,'on');
    xlabel(axes1,'Time (milliseconds)');
    ylabel(axes1,'Acceleration (m/s^2)');
    title(axes1,'Accelerometer B');
    hold(axes2,'on');
    xlabel(axes2,'Time (milliseconds)');
    ylabel(axes2,'Acceleration (m/s^2)');
    title(axes2,'Accelerometer A');
    %   Set axis ranges
    xmax = max(timevals);
    xmin = min(timevals);
    ymax1 = accel_settings(1)*9.81;
    ymin1 = -ymax1;
    ymax2 = accel_settings(2)*9.81;
    ymin2 = -ymax2;
    axis(axes2,[ xmin  xmax ymin1 ymax1]);
    axis(axes1,[ xmin  xmax ymin2 ymax2]);
%   Draw the legend    
    set(plot1(1),'DisplayName','ax');
    set(plot1(2),'DisplayName','ay');
    set(plot1(3),'DisplayName','az');
    set(plot2(1),'DisplayName','ax');
    set(plot2(2),'DisplayName','ay');
    set(plot2(3),'DisplayName','az');
    legend1 = legend(axes1,'show');
    set(legend1,'FontSize',9);
    legend2 = legend(axes2,'show');
    set(legend2,'FontSize',9);
%   Compute rms values    
    for i = 1:6
       amean = trapz(timevals,accelvals(:,i))/(xmax-xmin); % Mean
       accelvals(:,i) = accelvals(:,i)-amean; % Subtract the mean
       arms(i) = sqrt(trapz(timevals,accelvals(:,i).^2)/(xmax-xmin));
    end
    % Display the rms values on a textbox
    Axstr = ['x:  ',num2str(arms(1)),'  m/s^2'];
    Aystr = ['y:  ',num2str(arms(2)),'  m/s^2'];
    Azstr = ['z:  ',num2str(arms(3)),'  m/s^2'];
    Bxstr = ['x:  ',num2str(arms(4)),'  m/s^2'];
    Bystr = ['y:  ',num2str(arms(5)),'  m/s^2'];
    Bzstr = ['z:  ',num2str(arms(6)),'  m/s^2'];
    annotation(h,'textbox',...
    [0.0127845117845118 0.464037122969838 0.133680134680135 0.516357308584686],...
    'String',{'RMS AMPLITUDES','','Accelerometer A',Axstr,Aystr,Azstr,''...
    ,'Accelerometer B',Bxstr,Bystr,Bzstr},...
    'FitBoxToText','off',...
    'BackgroundColor',[1 1 1]);  
    drawnow;
end
close all
end

function [times,accelout] = read_accelerometer_history(accel_settings,dataport,newmat)
% Function to do a burst read of 100 acceleration datapoints from 2
% accelerometers
%
 if (newmat)
    cmd = sprintf("H");  % Send a character to the Arduino to request data
    writeline(dataport.s,cmd);
 else         
    fprintf(dataport.s,'H');   % Start the read
 end
 % Retreive the data
 times = zeros(100,1);
 accelout = zeros(100,3);
 %pause(75*accel_settings(3)/1000)
 if (newmat)
   for i = 1:100    
     r = readline(dataport.s);
     times(i) = sscanf(r,'%d');
     for j = 1:6
       r = readline(dataport.s);  
       accelout(i,j) = sscanf(r,'%d');
     end
   end
 else
     for i = 1:100
       times(i) = fscanf(dataport.s,'%d');
       accelout(i,1) = fscanf(dataport.s,'%d');
       accelout(i,2) = fscanf(dataport.s,'%d');
       accelout(i,3) = fscanf(dataport.s,'%d');
       accelout(i,4) = fscanf(dataport.s,'%d');
       accelout(i,5) = fscanf(dataport.s,'%d');
       accelout(i,6) = fscanf(dataport.s,'%d');  
     end
 end
 accelout(:,1:3) = accelout(:,1:3)*9.81*accel_settings(1)/2380;
 accelout(:,4:6) = accelout(:,4:6)*9.81*accel_settings(2)/2380;
end
function decrease_delaytime_Callback(hObject,eventdata)
% Button callback to change the delay time of ADXL362 accelerometer scan
% A button click will cycle through values  [1,2,4,8,16,32,64,1024]
% Calling this function will switch to the next higher setting
%
% Note that the callback does not update the accel_settings variable
% 
   data = guidata(hObject);
   newmat = data.NewMatlab;
   new_setting = data.DelayTime/2;
   if (new_setting <1) new_setting = 1; end
   data.DelayTime = new_setting;
   guidata(hObject,data);
   if (newmat)
    cmd = sprintf("C");  % Send a character to the Arduino 
    writeline(data.DataPort.s,cmd);
  else         
    fprintf(data.DataPort.s,'C');  % Send the command to the Arduino board
  end
   
end
function increase_delaytime_Callback(hObject,eventdata)
% Button callback to change the delay time of ADXL362 accelerometer scan
% A button click will cycle through values  [1,2,4,8,16,32,64,1024]
% Calling this function will switch to the next higher setting
%
% Note that the callback does not update the accel_settings variable
% 
   data = guidata(hObject);
   newmat = data.NewMatlab;
   new_setting = 2*data.DelayTime;
   if (new_setting > 1025) new_setting = 1024; end
   data.DelayTime = new_setting;
   guidata(hObject,data);
   if (newmat)
    cmd = sprintf("D");  % Send a character to the Arduino 
    writeline(data.DataPort.s,cmd);
  else         
    fprintf(data.DataPort.s,'D');  % Send the command to the Arduino board
  end
end
function A_sensitivity_Callback(hObject,eventdata)
% Button callback to change the sensitivity of accelerometer A
% A button click will cycle through values  [2,4,8,1]
% Calling this function will switch to the next higher setting
%
% Note that the callback does not update the accel_settings variable
% 
   data = guidata(hObject);
   newmat = data.NewMatlab;
   new_setting = 2*data.SensitivityA;
   if (new_setting > 8) new_setting = 1; end
   data.SensitivityA = new_setting;
   guidata(hObject,data);
   if (new_setting==2) return; end   % No need to change board to cycle from 1 to 2
   if (newmat)
    cmd = sprintf("S");  % SeP a character to the Arduino 
    writeline(data.DataPort.s,cmd);
  else         
    fprintf(data.DataPort.s,'S');  % Send the command to the Arduino board
  end
end
function B_sensitivity_Callback(hObject,eventdata)
% Button callback to change the sensitivity of accelerometer B
% A button click will cycle through values  [2,4,8,1]
% Calling this function will switch to the next higher setting
%
% Note that the callback does not update the accel_settings variable
% 
   data = guidata(hObject);
   newmat = data.NewMatlab;
   new_setting = 2*data.SensitivityB;
   if (new_setting > 8) new_setting = 1; end
   data.SensitivityB = new_setting;
   guidata(hObject,data);
   if (new_setting==2) return; end % No need to change board to cycle from 1 to 2
   if (newmat)
    cmd = sprintf("P");  % toggles arduino board to accel b
    writeline(data.DataPort.s,cmd);
    cmd = sprintf("S");  % Toggles sensitivity
    writeline(data.DataPort.s,cmd);   
    cmd = sprintf("P");  % back to a
    writeline(data.DataPort.s,cmd);
   else         
    fprintf(data.DataPort.s,'P'); % toggles arduino board to accel b
    fprintf(data.DataPort.s,'S'); % Toggles sensitivity
    fprintf(data.DataPort.s,'P'); % back to a
  end

end
function stop_button_Callback(hObject,eventdata)
   data = guidata(hObject);
   newmat = data.NewMatlab;
  closeSerial(newmat);
end
function new_settings = toggle_ADXL362_sensitivity(accel,accel_settings,dataport,newmat)
% Function to change the sensitivity of ADXL362 accelerometer
% Possible values are [+/- 2g (default), +/- 4g, +/- 8g]
% Calling this function will switch to the next higher setting
%
  if (accel=='a') 
    new_settings = accel_settings;
    if (accel_settings(1) == 2) new_settings(1) = 4; end
    if (accel_settings(1) == 4) new_settings(1) = 8; end
    if (accel_settings(1) == 8) new_settings(1) = 2; end
    if (newmat)
     cmd = sprintf("S");  % Send a character to the Arduino 
     writeline(dataport.s,cmd);
    else         
     fprintf(dataPort.s,'S');  % Send the command to the Arduino board
    end
  elseif (accel=='b')
    new_settings = accel_settings;
    if (accel_settings(2) == 2) new_settings(2) = 4; end
    if (accel_settings(2) == 4) new_settings(2) = 8; end
    if (accel_settings(2) == 8) new_settings(2) = 2; end
   if (newmat)
    cmd = sprintf("P");  % toggles arduino board to accel b
    writeline(dataport.s,cmd);
    cmd = sprintf("S");  % Toggles sensitivity
    writeline(dataport.s,cmd);   
    cmd = sprintf("P");  % back to a
    writeline(dataport.s,cmd);
   else         
    fprintf(dataPort.s,'P'); % toggles arduino board to accel b
    fprintf(dataPort.s,'S'); % Toggles sensitivity
    fprintf(dataPort.s,'P'); % back to a
  end
  else
    disp('Warning - invalid accelerometer selection')
    disp('Sensitivity was not changed')
  end
          
end
function new_settings = increase_delaytime(accel_settings,dataport,newmat)
% Function to change the delay time of ADXL362 accelerometer scan
% Possible values are [2,4,8,16,32,64]
% Calling this function will switch to the next higher setting
%
  new_settings = accel_settings;
  new_settings(3) = 2*accel_settings(3);
  if (new_settings(3)>65) new_settings(3) = 64; end
    if (newmat)
     cmd = sprintf("D");  % Send a character to the Arduino 
     writeline(dataport.s,cmd);
    else         
     fprintf(dataport.s,'D');  % Send the command to the Arduino board
    end
end
function new_settings = reduce_delaytime(accel_settings,dataport,newmat)
% Function to change the delay time of ADXL362 accelerometer scan
% Possible values are [2,4,8,16,32,64]
% Calling this function will switch to the next higher setting
%
  new_settings = accel_settings;
  new_settings(3) = accel_settings(3)/2;
  if (new_settings(3)<1) new_settings(3) = 1; end
  if (newmat)
   cmd = sprintf("C");  % Send a character to the Arduino 
   writeline(dataport.s,cmd);
  else         
   fprintf(dataport.s,'C');  % Send the command to the Arduino board
  end
end
function monitor_accels(accel_settings,dataport,newmat)
% Function to plot a continuous trace of acceleration-v-time
% The sample rate is severely limited by the serial port - the best
% that can be done is about a sample every 20 milliseconds
% Accel_settings are the values for accelerometer settings
% dataport is the serial dataport object
%
figure
if(~exist('h', 'var') || ~ishandle(h))
    h = figure(1);
    ax = axes('box','on');
end

if (~exist('button','var'))
    button = uicontrol('Style','pushbutton','String','Stop',...
        'pos',[0 0 50 25], 'parent',h,'CallBack',@stop_button_Callback,...
        'UserData',1);
end
%
buf_len = 100;
times = zeros(buf_len,1);
adata = zeros(buf_len,6);
adata(1) = 0.001;
c = clock;
tstart = c(6)+60*c(5)+3600*c(4);
%
while(get(button,'UserData'))
    [acca,accb] = read_instantaneous_acceleration(accel_settings,dataport,newmat);
    c = clock;
    time = c(6)+60*c(5)+3600*c(4)-tstart;
    times = [times(2:end);time];
    adata = [adata(2:end,1:3);acca;accb];
    if (get(button,'UserData'))
      plot(times,adata);
    end
    xmax = max(times);
    xmin = min(times);
    ymin = -max(accel_settings(1),accel_settings(2))*9.81;
    ymax = -ymin;
    axis([ xmin  xmax ymin ymax]);
    xlabel('Time (seconds)');
    ylabel('Acceleration (m/s^2)');
    drawnow;
end
close all
end
function [accelouta,acceloutb] = read_instantaneous_acceleration(accel_settings,dataport,newmat)
    % Function to read an instantaneous value of acceleration
    % accelout(1:3) is the acceleration vector (ax,ay,az)
    % dataport is the serial dataport object
    % Read accelerometer a
    if (newmat)
        cmd = sprintf("V");  % Send a character to the Arduino 
        writeline(dataport.s,cmd);
        for j = 1:3
            r = readline(dataport.s);
            accelouta(j) = sscanf(r,'%d');
        end
        cmd = sprintf("P");  % Toggles arduino to second accelerometer
        writeline(dataport.s,cmd);
        cmd = sprintf("V");  % Send a character to the Arduino 
        writeline(dataport.s,cmd);    
        for j = 1:3
            r = readline(dataport.s);
            acceloutb(j) = sscanf(r,'%d');
        end
        cmd = sprintf("P");  % Back to first accelerometer
        writeline(dataport.s,cmd);    
    else
        fprintf(dataport.s,'V');
        accelouta(1) = fscanf(dataport.s,'%d');
        accelouta(2) = fscanf(dataport.s,'%d');
        accelouta(3) = fscanf(dataport.s,'%d');
        % Read accelerometer b
        fprintf(dataport.s,'P');  % Toggles arduino to second accelerometer
        fprintf(dataport.s,'V');
        acceloutb(1) = fscanf(dataport.s,'%d');
        acceloutb(2) = fscanf(dataport.s,'%d');
        acceloutb(3) = fscanf(dataport.s,'%d');
        fprintf(dataport.s,'P'); % Back to first accelerometer
    end
    accelouta = accelouta*9.81*accel_settings(1)/2380;
    acceloutb = acceloutb*9.81*accel_settings(2)/2380;
end

