%% DC Motor Simulation
%This script simulates the control of the DC motors used in the project
%sequence for this class.

%Delvin E Peterson
%Code started 12-19-21
%Walla Walla University
%ENGR 350 - F21
%Project 4

clear variables
close all

%% Define the system
%Physical constants
Ra = 9.2;
La = 0.21e-3;
Kb = 0.014;
b = 2.5e-7;
J = 2e-6;

%Transfer functions
s = tf('s');
G = Kb/((J*s+b)*(La*s+Ra) + Kb^2) %Plant
Gu = 0.01;    %Unit conversion from rad/s to V
Op_gn = dcgain(G)

%% Try a proportional controller
ess = 0.01;
k = (1/ess-1) / (Op_gn*Gu)
k = 100*k

%Calculate the pole locations
T = feedback(k*G*Gu,1)
pole(T)

%% Try a PI controller
% Req. 1: ess < 1%
% Req. 2: umax < 10 V
% Req. 3: wc = 10 rad/s

% Use proportional control to meet 3 and integral control to meet 1, then
% see if 2 is met
bode(G*Gu)
grid on
hold on
% We need a gain of about 5.74 dB to meet Req. 3, wc = 10
k = 10^(5.74/20);
zi = 1; %Choose zi about wc/10
Gpi = k*(s+zi)/s
margin(Gpi*G*Gu)
% This meets requirements 1 and 3
kp = k
ki = k*zi
umax = 3*kp
% It works this year (5.8 < 10). Maybe I should increase the wc by tenfold.

%% Closer analysis
%Simulate the response
T = feedback(Gpi*G*Gu,1)
[y,t] = step(3*T);
u = lsim(Gpi,3-y,t);

%Plot the step response
figure
plot(t,y/Gu,'b-',[0 t(end)],[300 300],'g-.')
xlabel('Time (s)')
ylabel('Motor Speed (rad/s)')
title(['Controlled Step Response of DC Motor with kp = ' num2str(kp,3) ...
    ', and ki = ' num2str(ki,3)])
legend('Output Speed', 'Reference Speed')

%Plot the command response
figure
plot(t,u)
xlabel('Time (s)')
ylabel('Control Signal (V)')
title('Controller Values for PI Control of a DC Motor')

%Root locus plot
figure
rlocus(Gpi*G*Gu)
hold on
plot(real(pole(T)),imag(pole(T)),'+','MarkerSize',7)
title('Root Locus of DC Motor Control')

%Output resulting controller
disp(['G_pi = ' num2str(k,4) '(s + ' num2str(zi,4) ')/s   <=OR=>'])
disp(['G_pi = ' num2str(kp,4) ' + ' num2str(ki,4) '/s'])