import numpy as np
from matplotlib.pyplot import *
from scipy.integrate import odeint

def reponsePIDordre1(A0,T,Kp,Ti,Td,tmax,N,rtol=1e-4,atol=1e-4):
	Sc0 = 1
	def systeme(Y,t):
		dY0 = Y[1]
		dY1 = 1/(A0*Kp*Td+T)*(A0*Kp/Ti*Sc0-(A0*Kp+1)*Y[1]-A0*Kp/Ti*Y[0])
		return [dY0,dY1]
	temps = np.linspace(0,tmax,N)
	sol = odeint(systeme,[0,0],temps,rtol=rtol,atol=atol)
	return temps,sol[:,0]
				

def reponsePIDordre1_bis(A0,T,Kp,Kd,Ki,tmax,N,rtol=1e-4,atol=1e-4):
	return reponsePIDordre1(A0,T,Kp,Kp/Ki,Kd/Kp,tmax,N,rtol,atol)
				

def reponsePordre1(A0,T,Kp,tmax,N,rtol=1e-4,atol=1e-4):
	Sc0 = 1
	def systeme(Y,t):
		return 1/T*(A0*Kp*Sc0-(A0*Kp+1)*Y[0])
	temps = np.linspace(0,tmax,N)
	sol = odeint(systeme,[0],temps,rtol=rtol,atol=atol)
	return temps,sol[:,0]
				 

T = 1
A0 = 1
Kp = 100
Td = 0
tmax=1
N=1000
figure(figsize=(16,6))
title("T = 1 s, Kp = 100")
Ti = 0.01
t,S = reponsePIDordre1(A0,T,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$T_i=0,01\,\rm s$")
Ti = 0.1
t,S = reponsePIDordre1(A0,T,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$T_i=0,1\,\rm s$")
Ti = 1
t,S = reponsePIDordre1(A0,T,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$T_i=1\,\rm s$")
grid()
t,S = reponsePordre1(A0,T,Kp,tmax,N)
plot(t,S,label=r"$T_i=\infty$")
grid()
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()
				

T = 1
A0 = 1
Kp = 100
Td = 0.1
tmax=1
N=1000
figure(figsize=(16,6))
title("T = 1 s, Kp = 100")
Ti = 0.01
t,S = reponsePIDordre1(A0,T,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$T_i=0,01\,\rm s$")
Ti = 0.1
t,S = reponsePIDordre1(A0,T,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$T_i=0,1\,\rm s$")
Ti = 1
t,S = reponsePIDordre1(A0,T,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$T_i=1\,\rm s$")
grid()
t,S = reponsePordre1(A0,T,Kp,tmax,N)
plot(t,S,label=r"$T_i=\infty$")
grid()
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()
				

def reponsePordre2(A0,T,m,Kp,tmax,N,rtol=1e-4,atol=1e-4):
	Sc0 = 1
	def systeme(Y,y):
		dY0 = Y[1]
		dY1 = 1/T**2*(A0*Kp*Sc0-2*m*T*Y[1]-(1+A0*Kp)*Y[0])
		return [dY0,dY1]
	temps = np.linspace(0,tmax,N)
	sol = odeint(systeme,[0,0],temps,rtol=rtol,atol=atol)
	return temps,sol[:,0]
	
				

T = 1 
m = 5
A0 = 1
tmax = 10
N = 1000
figure(figsize=(16,6))
title("T = 1, m=5")
Kp = 10
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$K_p=10$")
Kp = 20
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$K_p=20$")
Kp = 50
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$K_p=50$")
Kp = 100
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$K_p=100$")
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()

				

T = 1 
m = 0.5
A0 = 1
tmax = 20
N = 1000
figure(figsize=(16,6))
title("T = 1, m=0,5")
Kp = 10
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$K_p=10$")
Kp = 50
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$K_p=50$")
Kp = 100
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$K_p=100$")
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()

				

def reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N,rtol=1e-4,atol=1e-4):
	Sc0 = 1
	def systeme(Y,y):
		dY0 = Y[1]
		dY1 = Y[2]
		dY2 = 1/T**2*(A0*Kp*(Sc0-Y[0])/Ti-(2*m*T+A0*Kp*Td)*Y[2]-(1+A0*Kp)*Y[1])
		return [dY0,dY1,dY2]
	temps = np.linspace(0,tmax,N)
	sol = odeint(systeme,[0,0,0],temps,rtol=rtol,atol=atol)
	return temps,sol[:,0]
	
				

T = 1 
m = 5
A0 = 1
tmax = 10
N = 1000
figure(figsize=(16,6))
title("T = 1, m=5")
Kp = 100
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$P : K_p=100$")
Kp = 100
Ti = 1
Td = 0
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PI : K_p=100,\ T_i=1$")
Ti = 0.5
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PI : K_p=100,\ T_i=0,5$")
Ti = 0.3
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PI : K_p=100,\ T_i=0,3$")
Ti = 0.1
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PI : K_p=100,\ T_i=0,1$")
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()

				

T = 1 
m = 5
A0 = 1
tmax = 10
N = 1000
figure(figsize=(16,6))
title("T = 1, m=5")
Kp = 100
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$P : K_p=100$")
Kp = 100
Td = 0
Ti = 0.05
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PI : K_p=100,\ T_i=0,05$")
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()

				

def Ti_limite(A0,T,m,Kp,Td):
	return (T**2*A0*Kp)/((2*m*T+A0*Kp*Td)*(1+A0*Kp))
T = 1 
m = 5
A0 = 1
Kp = 100
Ti_lim = Ti_limite(A0,T,m,Kp,Td)
		

T = 1 
m = 0.5
A0 = 1
Kp = 100
Td = 0
Ti_lim = Ti_limite(A0,T,m,Kp,Td)
		

T = 1 
m = 0.5
A0 = 1
tmax = 10
N = 1000
figure(figsize=(16,6))
title("T = 1, m=0,5")
Kp = 100
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$P : K_p=100$")
Kp = 100
Ti = 5
Td = 0
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PI : K_p=100,\ T_i=5$")
Ti = 1
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PI : K_p=100,\ T_i=1$")
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()

				

T = 1 
m = 5
A0 = 1
Kp = 100
Td = 1
Ti_lim = Ti_limite(A0,T,m,Kp,Td)
		

T = 1 
m = 5
A0 = 1
tmax = 10
N = 1000
figure(figsize=(16,6))
title("T = 1, m=5")
Kp = 100
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$P : K_p=100$")
Kp = 100
Ti = 1
Td = 1
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PID : K_p=100,\ T_i=1,\ T_d=1$")
Ti = 0.5
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PID : K_p=100,\ T_i=0,5,\ T_d=1$")
Ti = 0.3
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PID : K_p=100,\ T_i=0,3,\ T_d=1$")
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()

				

T = 1 
m = 0.5
A0 = 1
Kp = 100
Td = 1
Ti_lim = Ti_limite(A0,T,m,Kp,Td)
		

T = 1 
m = 0.5
A0 = 1
tmax = 10
N = 1000
figure(figsize=(16,6))
title("T = 1, m=0,5")
Kp = 100
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$P : K_p=100$")
Kp = 100
Ti = 5
Td = 1
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PID : K_p=100,\ T_i=5,\ T_d=1$")
Ti = 1
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PID : K_p=100,\ T_i=1,\ T_d=1$")
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()

				

T = 1 
m = 0.5
A0 = 1
Kp = 100
Td = 0.5
Ti_lim = Ti_limite(A0,T,m,Kp,Td)
		

T = 1 
m = 0.5
A0 = 1
tmax = 10
N = 1000
figure(figsize=(16,6))
title("T = 1, m=0,5")
Kp = 100
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$P : K_p=100$")
Kp = 100
Ti = 0.5
Td = 0.5
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PID : K_p=100,\ T_i=0,5,\ T_d=0,5$")
Ti = 0.2
Td = 0.2
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PID : K_p=100,\ T_i=0,2,\ T_d=0,2$")
Ti = 0.1
Td = 0.1
t,S = reponsePIDordre2(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PID : K_p=100,\ T_i=0,1,\ T_d=0,1$")
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()

				

from scipy.signal import residue

def decompositionPIDordre2(A0,T,m,Kp,Ti,Td):
	b = [A0*Kp*Td,A0*Kp,A0*Kp/Ti]
	a = [T**2,2*m*T+A0*Kp*Td,1+A0*Kp,A0*Kp/Ti]
	return residue(b,a,tol=1e-4)
				 

A0 = 1
T = 1
m = 0.5
Kp = 100
Ti = 0.2
Td = 0.2
[r,p,d] = decompositionPIDordre2(A0,T,m,Kp,Ti,Td)
				 

from numpy.linalg import solve
A = np.array([[1,1,1],[p[0],p[1],p[2]],[p[0]**2,p[1]**2,p[2]**2]])
B = np.array([-1,0,0])
a = solve(A,B)
				 

t = np.linspace(0,10,1000)
S = 1 + np.real(a[0]*np.exp(p[0]*t)+a[1]*np.exp(p[1]*t)+a[2]*np.exp(p[2]*t))
figure(figsize=(16,6))
plot(t,S)
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
grid()
				 

def reponsePIDordre2poles(A0,T,m,Kp,Ti,Td,tmax,N):
	[r,p,d] = decompositionPIDordre2(A0,T,m,Kp,Ti,Td)
	A = np.array([[1,1,1],[p[0],p[1],p[2]],[p[0]**2,p[1]**2,p[2]**2]])
	B = np.array([-1,0,0])
	a = solve(A,B)
	t = np.linspace(0,tmax,N)
	S = 1 + np.real(a[0]*np.exp(p[0]*t)+a[1]*np.exp(p[1]*t)+a[2]*np.exp(p[2]*t))
	return t,S
				 

A0 = 1
T = 1
m = 0.5
Kp = 100
Ti = 0.2
Td = np.linspace(0.01,0.5,30)
figure()
title("Kp=100, Ti = 0,2")
style =  ["ro","b+","g."]
for k in range(len(Td)):
	[r,p,d] = decompositionPIDordre2(A0,T,m,Kp,Ti,Td[k])
	for i in range(3):
		plot(Td[k],-np.real(p[i]),style[i])
xlabel(r"$T_d$",fontsize=16)
ylabel("-Re(p)")

grid()
				 

yscale('log')
				 

T = 1 
m = 0.5
A0 = 1
tmax = 10
N = 1000
figure(figsize=(16,6))
title("T = 1, m=0,5")
Kp = 100
t,S = reponsePordre2(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$P : K_p=100$")
Ti = 0.2
Td = 0.1
t,S = reponsePIDordre2poles(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PID : K_p=100,\ T_i=0,2,\ T_d=0,1$")
Ti = 0.2
Td = 0.12
t,S = reponsePIDordre2poles(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PID : K_p=100,\ T_i=0,2,\ T_d=0,12$")
Ti = 0.2
Td = 0.2
t,S = reponsePIDordre2poles(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PID : K_p=100,\ T_i=0,2,\ T_d=0,2$")
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()

				

def decompositionPordre2(A0,T,m,Kp):
	b = [A0*Kp]
	a = [T**2,2*m*T,1+A0*Kp]
	return residue(b,a,tol=1e-4)
				 

def reponsePordre2poles(A0,T,m,Kp,tmax,N):
	[r,p,d] = decompositionPordre2(A0,T,m,Kp)
	A = np.array([[1,1],[p[0],p[1]]])
	B = np.array([-A0*Kp/(1+A0*Kp),0])
	a = solve(A,B)
	t = np.linspace(0,tmax,N)
	S = A0*Kp/(1+A0*Kp) + np.real(a[0]*np.exp(p[0]*t)+a[1]*np.exp(p[1]*t))
	return t,S
				 

def reponseOrdre2poles(A0,T,m,tmax,N):
	b = [A0]
	a = [T**2,2*m*T,1]
	[r,p,d] = residue(b,a,tol=1e-4)
	A = np.array([[1,1],[p[0],p[1]]])
	B = np.array([-1,0])
	a = solve(A,B)
	t = np.linspace(0,tmax,N)
	S = 1 + np.real(a[0]*np.exp(p[0]*t)+a[1]*np.exp(p[1]*t))
	return t,S
				 

m=0.5
T = 1e-3
A0 = 1
tmax = 0.05
N = 1000
t,S = reponseOrdre2poles(A0,T,m,tmax,N)
figure(figsize=(16,6))
plot(t,S)
grid()
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{A_0}$",fontsize=16)
				  

tmax = 0.05
N = 1000
figure(figsize=(16,6))
title("T = 1e-3, m=0,5")
Kp = 10
t,S = reponsePordre2poles(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$P : K_p=10$")
Kp = 100
t,S = reponsePordre2poles(A0,T,m,Kp,tmax,N)
plot(t,S,label=r"$P : K_p=100$")
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()

				

Kp = np.linspace(1,50,100)
Ti_lim = Ti_limite(A0,T,m,Kp,0)
figure(figsize=(12,6))
plot(Kp,Ti_lim)
grid()
xlabel(r"$K_p$",fontsize=16)
ylabel(r"$T_i\ (\rm min)$",fontsize=16)
yscale('log')
				 

Kp = np.logspace(-1,2,20)
Ti = 1e-3
Td = 0
figure()
title("Ti = 1e-3")
style =  ["ro","b+","g."]
for k in range(len(Kp)):
	[r,p,d] = decompositionPIDordre2(A0,T,m,Kp[k],Ti,0)
	for i in range(3):
		plot(Kp[k],-np.real(p[i]),style[i])
xlabel(r"$K_p$")
ylabel("-Re(p)")
yscale('log')
xscale('log')
grid()
				 

tmax = 0.05
N = 1000
figure(figsize=(16,6))
title("T = 1e-3, m=0,5")
Kp=0.1
Ti=1e-3
Td=0
t,S = reponsePIDordre2poles(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PI : K_p=0,1, T_i=1e-3$")
Kp=0.5
Ti=1e-3
Td=0
t,S = reponsePIDordre2poles(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PI : K_p=0,5, T_i=1e-3$")
Kp=1
Ti=1e-3
Td=0
t,S = reponsePIDordre2poles(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PI : K_p=1, T_i=1e-3$")
Kp=10
Ti=1e-3
Td=0
t,S = reponsePIDordre2poles(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PI : K_p=10, T_i=1e-3$")
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()

				

Kp = 0.5
Ti = 1e-3
Td = np.logspace(-4,-2,100)
figure()
title("Kp = 0,5, Ti = 1e-3")
style =  ["ro","b+","g."]
for k in range(len(Td)):
	[r,p,d] = decompositionPIDordre2(A0,T,m,Kp,Ti,Td[k])
	for i in range(3):
		plot(Td[k],-np.real(p[i]),style[i])
xlabel(r"$T_d$")
ylabel("-Re(p)")
yscale('log')
xscale('log')
grid()
				 

tmax = 0.05
N = 1000
figure(figsize=(16,6))
title("T = 1e-3, m=0,5")
Kp=0.5
Ti=1e-3
Td=1.8e-3
t,S = reponsePIDordre2poles(A0,T,m,Kp,Ti,Td,tmax,N)
plot(t,S,label=r"$PI : K_p=0,5, T_i=1e-3, Td=1,8e-3$")
xlabel("t (s)",fontsize=16)
ylabel(r"$\frac{S}{S_c^0}$",fontsize=16)
legend(loc="lower right")
grid()