% Jeff Sarsona, Saba Fazeli
% E105 Mini Project
% Electronic Throttle Control

clear all; close all; clc

s = tf('s');
K1 = 1*10^5;   % root locus of OL, found where sys was most stable
J = 1.53e-5;   % moment of inertia of disk
b = 0.03;      % damping constant [Ns/m]
k = 2e-3;      % spring constant [N/m]

ThetaIn = 15;
ang2torq = 1.59/90;
TorqueIn = ang2torq*ThetaIn; %N
G = 1/(s^2+b/J*s+k/J); %theta_valve/T_motor
opt = stepDataOptions('StepAmplitude', TorqueIn); %step input = 0.5 N*m
%rlocus(G)
%figure
%step(G,opt)

%Low pass filter (may or may not be necessary)
numlpf = [1];
denlpf = [2 1];
lpf = tf(numlpf,denlpf);


% Controller D - Lead compensator
PM = 40.0;
wc = 0.23; 
phi_deg = PM - 2.5 + 15 %%add ten for safety factor 
phimax = phi_deg*(2*pi/360);
a = (1-sin(phimax))/(1+sin(phimax))
T = 1/(wc*sqrt(a))
b = a*T;
numD = [T 1];
denD = [b 1];
klead = .3;
D = tf(klead*numD,denD);
% Controller D1 - Lag compensator
alag = .5;
Tlag = 10;
blag = alag*Tlag;
numD1 = alag*[Tlag 1];
denD1 = [blag 1];
D1 = tf(numD1,denD1);

Dcl = feedback(K1*D1*D*G,1)
degreesOut = 360*Dcl/(2*pi)
%figure
%bode(Dcl)
figure
step(Dcl,opt)
title('Closed-loop Step Response');
xlabel('Time');
ylabel('Butterfly Valve Angle (rad)');

figure
step(degreesOut,opt)
title('Closed-loop Step Response');
xlabel('Time');
ylabel('Butterfly Valve Angle (deg)');

w_BW = 20; % rad/s
ws = 25*w_BW;
fs = ws/(2*pi); %sec^-1
Tsample = 1/fs; %sec
Dd = c2d(degreesOut,Tsample,'tustin')

figure
step(Dd, opt)
title('System Step Response with Digitized Controller (Tustin Approx.)');
xlabel('Time');
ylabel('Butterfly Valve Angle (deg)');

%rlocus(Dcl);
%figure
%bode(Dcl)
%figure
%step(D*G)