##################################################################################################
## Modélisation Python d'un asservissement discretisé
## Ce document permet de prendre en main et de comprendre petit à petit comment ecrire un fichier Python permettant de simuler le comportement d'un systeme de maniere discrete.
##################################################################################################

import math as ma
import matplotlib.pyplot as plt
import numpy as np


## Conditions initiales

T = 1e-1
K = 1
tau = 3
y0 = 0


t_0 = 1
t_1 = 10

t = np.linspace(0,t_0+t_1,(t_0+t_1)/T)
ech = np.concatenate((np.zeros(int(t_0/T),dtype=int),np.ones(int(t_1/T),dtype=int)),axis=0)
u = ech

##################################################################################################
## Partie 1 : boucle ouverte
##################################################################################################

def BO_1Ordre_z(K,tau,T,y0,u):
	y=[y0]
	for k in range(len(u)-1):
		y.append(y[k]*ma.exp(-T/tau) + K*u[k]*(1-ma.exp(-T/tau)))
	return(y)

plt.figure("Réponse à un échelon d'un système en BO") # Définition d'une nouvelle figure
plt.plot(t,u,'o') # Graphe
plt.plot(t,BO_1Ordre_z(K,tau,T,y0,ech),'o') # Graphe
plt.xlabel("Temps [s]") # Légende de l'axe des abscisses
plt.ylabel("Signal [1]") # Légende de l'axe des ordonnées
plt.grid() # Affiche une grille
plt.show() # Affichage (du buffer) de la figure
plt.legend()

##########################################"
def quantification(u,y,t):
	u2=[u[0]]
	y2=[y[0]]
	t2=[t[0]]
	for k in range(1,len(u)):
		u2.append(u[k-1])
		u2.append(u[k])
		y2.append(y[k-1])
		y2.append(y[k])
		t2.extend([t[k],t[k]])
	return(u2,y2,t2)

u2 ,y2 ,t2 = quantification (u, BO_1Ordre_z (K,tau ,T,y0 ,u),t)

# plt.figure ("Réponse  à un échelon  d’un  système en BO") # Dé finition d’une nouvelle figure
# plt. plot (t2 ,u2) # Graphe
# plt. plot (t2 ,y2) # Graphe
# plt. xlabel (" Temps  [s]") # Lé gende de l’axe des abscisses
# plt. ylabel (" Signal  [1]") # Lé gende de l’axe des ordonn ées
# plt. grid () # Affiche une grille
# plt. show () # Affichage (du buffer ) de la figure

##################################################################################################
## Partie 2 : boucle fermee
##################################################################################################
def BF_1Ordre_z(K,tau,T,y0,yc):
	y=[y0]
	for k in range(len(yc)-1):
		y.append( y[k]*( (1+K)*ma.exp(-T/tau)-K ) + yc[k]*K*(1-ma.exp(-T/tau)))
	return(y)

def BF_1Ordre_z2(K,tau,T,y0,yc):
	y=[y0]
	eps=[yc[0]]
	for k in range(len(yc)-1):
		eps.append(yc[k+1]-y[k])
		y.append(y[k]*ma.exp(-T/tau) + K*eps[k]*(1-ma.exp(-T/tau)))
	return(y)


#plt.figure("Réponse à un échelon discrétisé") # Définition d'une nouvelle figure
#plt.plot(t,ech,'o') # Graphe
#plt.plot(t,BF_1Ordre_z(K,tau,T,y0,ech),'o') # Graphe
#plt.plot(t,BF_1Ordre_z2(K,tau,T,y0,ech),'o') # Graphe
#plt.plot(t,BO_1Ordre_z(K,tau,T,y0,ech),'o') # Graphe
#plt.xlabel("Temps [s]") # Légende de l'axe des abscisses
#plt.ylabel("Signal [1]") # Légende de l'axe des ordonnées
#plt.grid() # Affiche une grille


u2,y2,t2 =quantification(u,BO_1Ordre_z(K,tau,T,y0,ech),t)
u3,y3,t3 =quantification(u,BF_1Ordre_z(K,tau,T,y0,ech),t)

#plt.figure("Réponse à un échelon quantifié") # Définition d'une nouvelle figure
#plt.plot(t2,u2,label='entrée echelon') # Graphe
#plt.plot(t3,y3,label='BF') # Graphe
#plt.plot(t2,y2,label='BO') # Graphe
#plt.xlabel("Temps [s]") # Légende de l'axe des abscisses
#plt.ylabel("Signal [1]") # Légende de l'axe des ordonnées
#plt.grid() # Affiche une grille
#plt.legend()
#plt.show()


##################################################################################################
## Partie 3 : boucle fermee avec correcteur PID
##################################################################################################

def PID(T,eps,int_eps,Kp,Ki,Kd,u):
	eps_k = eps[-1]
	eps_km1 = eps[-2]
	int_eps += eps_k*T
	d_eps = (eps_k - eps_km1)/T
	u.append(Kp*eps_k + Ki*int_eps + Kd*d_eps )
	return(u,int_eps)


def BF_1Ordre_PID_z(K,tau,T,y0,yc,Kp,Ki,Kd):
	y=[y0]
	eps=[yc[0]-y[0]]
	u=[Kp*eps[0]]
	int_eps=0
	d_eps=0
	y.append(y[0]*ma.exp(-T/tau) + K*u[0]*(1-ma.exp(-T/tau)))
	for k in range(1,len(yc)-1):
		eps.append(yc[k]-y[k])
		u,int_eps=PID(T,eps,int_eps,Kp,Ki,Kd,u)
		y.append(y[k]*ma.exp(-T/tau) + K*u[k]*(1-ma.exp(-T/tau)))
	return(y)


#plt.figure("Réponse à un échelon discrétisé") # Définition d'une nouvelle figure
#plt.plot(t,ech,'o') # Graphe
#plt.plot(t,BO_1Ordre_z(K,tau,T,y0,ech),'o') # Graphe
#plt.plot(t,BF_1Ordre_z(K,tau,T,y0,ech),'o') # Graphe
#plt.plot(t,BF_1Ordre_z2(K,tau,T,y0,ech),'o') # Graphe
#plt.plot(t,BF_1Ordre_PID_z(K,tau,T,y0,ech,Kp,Ki,Kd),'o') # Graphe
#plt.xlabel("Temps [s]") # Légende de l'axe des abscisses
#plt.ylabel("Signal [1]") # Légende de l'axe des ordonnées
#plt.grid() # Affiche une grille

# Parametres du correcteur
Kp = 8
Ki = 1
Kd = 1

ePID,sPID,tPID = quantification(ech,BF_1Ordre_PID_z(K,tau,T,y0,ech,Kp,Ki,Kd),t)

plt.figure("Réponse à un échelon avec correcteur PID - dicrétisé") # Définition d'une nouvelle figure
plt.plot(tPID,ePID,label='entrée échelon') # Graphe
plt.plot(t2,y2,label='BO') # Graphe
plt.plot(t3,y3,label='BF') # Graphe
plt.plot(tPID,sPID,label='BF avec PID') # Graphe
plt.xlabel("Temps [s]") # Légende de l'axe des abscisses
plt.ylabel("Signal [1]") # Légende de l'axe des ordonnées
plt.grid() # Affiche une grille
plt.legend()
plt.show() # Affichage (du buffer) de la figure