%%openlooptest
% LSA project day 4 script - Open loop test. This script will run the motor
% at a fixed value of voltage for a short period of time. It will also
% measure and display the motor speed.
%
% Delvin E Peterson
% Walla Walla University
% ENGR 350 - Winter 2020
% Project Day 4
% Based on previous code by Ralph Stirling

clear variables, close all
global sample_period V_max      %Global values are available to subfunctions

sample_period = 1/100;			%length of sample time interval

%% Changeable paramters
V_max = 10;         %This should be set to the voltage displayed on your multimeter
V_in = 3;         %Choose any voltage between +/-V_max
run_time = 1;       %Run time in seconds

%% Set up input voltages and time vectors
len = run_time/sample_period + 1;	%Length of run in samples
if abs(V_in)>V_max                  %Cap the voltage at the max
    V_in = V_max*sign(V_in);
end
volts = V_in*ones(1,len);			%Step up to the desired voltage
volts(len) = 0;                     %Turn it off at the end

t_exp = 0:sample_period:run_time;   %Time data SHOULD be collected at
t_act = t_exp;                      %Time data WERE collected at (set later)
enc = zeros(1,len);                 %Pre-allocate the motor encoder data

%% Run the test
% The motor is driven by an H-bridge circuit and pulse width modulation.
serial_start;

enc(1) = read_enc();
write_volt(volts(1));
tic
for cnt=2:len
	enc(cnt) = read_enc();          %Read encoder counts since last sample period
    write_volt(volts(cnt));         %Write voltage value to controller
	t_act(1,cnt) = toc;             %Store actual time elapsed
end

serial_stop;

% plot(t_exp,t_act,'b-',t_exp,t_exp,'g:') %Debug time stepping

%% Calculate and plot results
[spd_rps,spd_rpm] = motor_speed(t_act,enc);
figure,subplot(2,1,1)

plot(t_act(2:len),spd_rps)
title(['Motor Response Speed to an Input Voltage of ' num2str(V_in,3) ' V'])
ylabel('Motor Speed (rad/s)')
ymx = mean(spd_rps)*1.5;
ylim([-abs(ymx) abs(ymx)])

%Add a digital filter
filter_step = 10;
spd_rps_f = spd_rps;
for cnt = 1:len-1
    if cnt <= filter_step
        spd_rps_f(cnt) = mean(spd_rps(1:cnt));
    else
        spd_rps_f(cnt) = mean(spd_rps(cnt-filter_step:cnt));
    end
end
hold on
plot(t_act(2:len),spd_rps_f,'c-')
legend('Unfiltered','Digital Filter','Location','SouthOutside')

subplot(2,1,2)
plot(t_act(2:len),spd_rpm)
xlabel('Time (s)')
ylabel('Motor Speed (rpm)')
ymx = mean(spd_rpm)*1.5;
ylim([-abs(ymx) abs(ymx)])