#A script to illustrate the incubator P/PI/PID control computations for Section 5.6.

#System/Plant Model: The uncontrolled incubator temperature is governed by the
#Newton cooling ODE y'(t) = -k*(y(t) - a(t)) where "k" is the cooling constant
#and "a(t)" is ambient temperature. We let us take, for the moment,
k = 0.05;
var('t');
a(t) = 0;

#The control function will be u(t), and the constant "K" in the controlled ODE
#(equation (5.108) in the text) will be K = 1. The controlled ODE is thus
#y'(t) = -k*(y(t)-a(t)) + K*u(t) with
K = 1;

#The desired temperature (setpoint) will be 0 degrees for t < 20 and then 3
#degrees for t > 20.
r(t) = 3*heaviside(t-20);

#Assume the initial condition is y(0) = y0 with
y0 = 5;

#The Control: Choose the control gains for PID control. In this case we will use
#PI control (so Kd = 0):
Kp = 1; #Proportional gain
Ki = 1/10; #Integral gain
Kd = 0; #Derivative gain

#The plant transfer function Gp(s) and controller transfer Gc(s) are, from (5.111)
#and (5.129) respectively,
var('s');
Gp(s) = K/(s+k);
Gc(s) = Kp + Ki/s + Kd*s;

#From (5.118) the closed-loop transfer function is
G(s) = Gp(s)*Gc(s)/(1+Gp(s)*Gc(s));

#The System Response to a Disturbance: This system starts off at the wrong
#temperature (5 degrees instead of the desired setpoint 0) and there is a
#disturbance in the form of an abrupt setpoint change at time t = 20.

#The governing ODE is y'(t) = -k(y(t) -a(t)) + K*u(t) where u(t) = r(t)  - y(t).
#According to equation (5.133) the system response in the s domain can be
#computed as Y(s) = G(s)*R(s) + y0*Gp(s)/(1+Gp(s)*Gc(s)). Using Sage:
R = laplace(r(t),t,s,algorithm='sympy');
Y = simplify(G(s)*R[0] + y0*Gp(s)/(1+Gp(s)*Gc(s)));

#The time domain response is then
ysol(t) = inverse_laplace(Y,s,t,algorithm='sympy');
print(ysol);

#A plot
plt1 = plot(ysol(t), (t,0,100),color='red');
plt2 = plot(r(t), (t,0,100),color='blue');
show(plt1+plt2)

#Note the system starts off at 5 degrees (where the desired setpoint is zero),
#so the control cools the system to 0 degrees, with a bit of overshoot. The
#controller effectively responds to the change in the setpoint at time t = 20.