%%Project4_PIControl
% LSA project day 4 script - Proporitional, and integral control test. This
% script will implement a PI controller for the speed of a DC motor.
%
% 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;         %Set to the voltage displayed on your multimeter
Kp = .06;           %Proportional control constant
Ki = 0;             %Integral control constant
om_des = 300;       %Desired output speed
run_time = 5;       %Run time in seconds
filter_step = 10;   %Increasing this will smooth out response, but also slow initial response

%% Set up input voltages and time vectors
len = run_time/sample_period + 1;	%Length of run in samples

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
spd_rps = zeros(1,len);             %Pre-allocate the motor speed
spd_rps_f = spd_rps;                %Pre-allocate filtered motor speed
om_int = 0;                         %Set the integral of speed to start at 0
Vp = zeros(1,len);                  %Pre-allocate the proportional signal
Vi = zeros(1,len);                  %Pre-allocate the integral signal
V_in = Vp+Vi;                       %Pre-allocate the total input voltage

%% 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
    t_act(1,cnt) = toc;             %Store actual time elapsed
    om_act = motor_speed(t_act(cnt-1:cnt),enc(cnt-1:cnt));
    spd_rps(cnt) = om_act;
    % Implement digital filter
    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
    om_act_f = spd_rps_f(cnt);
    %
    Vp(cnt) = Kp*(om_des - om_act_f);   %Calculate proportional signal
    om_int = om_int + (om_des - om_act_f)*(t_act(cnt) - t_act(cnt-1));
    Vi(cnt) = Ki*om_int;                %Calculate integral signal
    V_in(cnt) = Vp(cnt) + Vi(cnt);
    if abs(V_in(cnt)) > V_max           %Cap the voltage at the max
        V_in(cnt) = V_max*sign(V_in(cnt));
    end
    write_volt(V_in(cnt));         %Write voltage value to controller
end
write_volt(0);
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,spd_rps,'b-',t_act,spd_rps_f,'c-')
title(['Motor Response Speed to PI Control with Kp = ' num2str(Kp,3) ...
    ' and Ki = ' num2str(Ki,3)])
ylabel('Motor Speed (rad/s)')
ymx = mean(spd_rps)*2;
ylim([-abs(ymx) abs(ymx)])
legend('Unfiltered','Digital Filter','Location','SouthOutside')

subplot(2,1,2)
plot(t_act,Vp,'b-.',t_act,Vi,'r-.',t_act,V_in,'g-')
xlabel('Time (s)')
ylabel('Control Signal (V)')
legend('Proportional Signal','Integral Signal','Total Signal',...
    'Location','SouthOutside')