%Use this script to read the speed data of a motor encoder while the motor
%is run by an external voltage. By setting the external voltage to a
%sinusoidal wave form, you can collect data on speed amplitude, and use
%this data to create a Bode plot of the unloaded DC motor. You can use
%this information along with a step response to refine our model
%parameters.

%Delvin E Peterson
%Walla Walla University
%ENGR 350 - Winter 2019
%Project Day 3
%Started 3-1-19

clear variables
close all
global ser sample_period;

%Time parameters of the data to collect
sample_period = 1/100;			% length of sample time interval
%%% Vary these two parameters as needed to show good data on the plot
run_time = 5;                   % total time to collect data
ylims = 300;                   % Max motor speed for plotting
%%%

t_exp = 0:sample_period:run_time;%Time increments that data SHOULD be collected at
t_act = t_exp;                  % time increments that data WERE collected at (later code will overwrite values)
ind_end = length(t_exp);        % length of data to be taken
enc = zeros(1,ind_end);         % pre-allocate the motor encoder data

serial_start    %Starts the microprocessor's algorithm
tic
for cnt = 1:ind_end
    enc(1,cnt) = read_enc();    %Read encoder counts since last sample period
    t_act(1,cnt) = toc;         %Store actual time elapsed
end
%t_act(1)
t_act = t_act - t_act(1);

% % Figures used for debugging sample rate
% figure
% plot(t_exp(1:20),t_act(1:20))
% 
% figure
% plot(t_exp,t_act-t_exp)

% Converter encoder data to motor speed in rad/s
% Both edges of two different pulses from the encoder are counted, and the 
% encoders pulse 48 times per revolution,
% giving 48*4=192 counts per revolution.
spd_Hz = diff(enc)/(48*4)./diff(t_act);
%NOTE: this is a crude first order numerical differentiation, a better
%algorithm could also be used. This algorithm returns one less data point.
spd_rps = spd_Hz*2*pi;

figure
plot(t_act(3:end),spd_rps(2:end))   %The first data point is always unreasonable ???
title('Motor Speed During Sample - With External Voltage Input')
xlabel('Time (s)')
ylabel('Motor Speed (rad/s)')
ylim([-ylims ylims])