''' Euler - Asservissement numérique - Période d'échantillonage - Correcteurs numériques '''

## Exercice 1: Simulation d'un MCC

# Question 1:

'''
Cf corrigé pdf
di = (u-ke*w-R*i)/L
dw = (kc*i-f*w-cr)/J
'''

# Question 2: Résolution

def Euler_Explicite(f,y0,t0,t1,dt):
    t = t0
    y = y0
    T = [t]
    Y = [y]
    i = 0
    while t < t1: # Important: < et non <=
        yp = f(y,t)
        y = [y[i] + yp[i]*dt for i in range(len(y))]
        i += 1
        t = t0 + i*dt # Important: Eviter les erreurs de t+=dt
        T.append(t)
        Y.append(y)
    return T,Y

def F(Y,t): # u variable globale
    R = 0.45
    L = 0.0046
    ke = 0.169
    kc = 0.17
    f = 0.01
    J = 0.01
    cr = 0
    i,w = Y
    di = (u-ke*w-R*i)/L
    dw = (kc*i-f*w-cr)/J
    Sol = [di,dw]
    return Sol

i0 = 0
w0 = 0
V0 = [i0,w0]
t0 = 0
t1 = 1
N = 1000
u = 10

dt = (t1-t0)/(N-1)
Lt,Y = Euler_Explicite(F,V0,t0,t1,dt)
Li = [Y[i][0] for i in range(len(Y))]
Lw = [Y[i][1] for i in range(len(Y))]

# Question 3: Tracé
from IPython.display import set_matplotlib_formats
set_matplotlib_formats('svg')
import matplotlib.pyplot as plt
plt.close('all')
def f_Affiche(fig,Lx,Ly,Legende):
    plt.figure(fig)
    plt.plot(Lx,Ly,label=Legende)
    plt.legend()
    plt.show()
    plt.pause(0.0001)

f_Affiche(1,Lt,Li,'i')
f_Affiche(1,Lt,Lw,'w')
#%%
# Question 4: Fonction MCC

def MCC(V0,t0,t1,dt):
    Lt,Y = Euler_Explicite(F,V0,t0,t1,dt)
    Li = [Y[i][0] for i in range(len(Y))]
    Lw = [Y[i][1] for i in range(len(Y))]
    return Lt,Li,Lw

# Question 5: Nombre de points pour une réponse fiable

for N in [1000,5000,10000,100000]:
    dt = (t1-t0)/(N-1)
    Lt,Li,Lw = MCC(V0,t0,t1,dt)
    f_Affiche(2,Lt,Li,str(N)+' i')
    f_Affiche(2,Lt,Lw,str(N)+'w')

'''
On voit peu de différence entre 5000, 10 000 et 100 000 points
'''

# Question 6: dts

N = 5000
dts = (t1-t0)/(N-1)
print('dts: ',dts)

'''
dts environ égal à 2.10^-4
'''

# Question 8 - odeint

Lt = [t0+i*dts for i in range(N)]
from scipy.integrate import odeint
Sol = odeint(F,V0,Lt)
Li = [Sol[i][0] for i in range(len(Sol))]
Lw = [Sol[i][1] for i in range(len(Sol))]
f_Affiche(3,Lt,Li,'i')
f_Affiche(3,Lt,Lw,'w')

# Question 8: cf corrigé pdf

#%%
## Exercice 2:  Simulation d’asservissement du MCC

# Question 1: Résolution

# Données de l'asservissement

Wc = 1     # Consigne (rd/s)
Kcapt = 10 # Gain capteur (V/rd/s)
Ka = Kcapt # Adaptateur (V/rd/s)
Uc = Ka*Wc # Consigne au comparateur (V)

# Données numériques

dts = 0.0002 # Pas de temps résolution MCC (s)
ti = 0       # Temps initial simulation
tf = 0.2     # Temps final simulation
n = 5        # Facteur sur le pas de temps utile pour les 2 objectifs
dt = n*dts   # Pas de temps de simulation

# Initialisation des données

i0 = 0
w0 = 0
u0 = 0
Li = [i0]
Lw = [w0]
Lu = [u0]
Lt = [ti]
Ltu = [ti]

# Boucle de simulation

k = 0 # Ne  pas mettre i
t = ti
while t < tf:
    i = Li[-1]
    w = Lw[-1]
    V = [i,w]
    k += 1
    t = ti + k*dt # Pas très important ici par rapport à t+=dt
    r = w * Kcapt
    eps = Uc - r
    u = eps # On mettra un correcteur ici
    Ltu.append(t)
    Lu.append(u)
    lt,li,lw = MCC(V,t,t+dt,dts)
    Li += li
    Lw += lw
    Lt += lt

def Affiche_Creneaux(fig,X,Y):
    plt.figure(fig)
    for i in range(len(X)-2):
        plt.plot(X[i:i+2],[Y[i+1],Y[i+1]])
    plt.show()

f_Affiche(4,Lt,Li,'i')
f_Affiche(4,Lt,Lw,'w')
Affiche_Creneaux(4,Ltu,Lu)

# Question 3:

'''
En changeant n ci-dessus, on montre l'apparition de retard et d'instabilité
'''
#%%
## Exercice 3: Correcteurs numériques

# Question 1: cf. pdf

# Question 2: Fonctions

def CP(ek,Kp):
    uk = Kp*ek
    return uk

def CPI(ukm,ek,ekm,Te,Kp,Ki):
    A = 1
    B = Kp+Te*Ki
    C = Kp
    uk = A*ukm + B*ek - C*ekm
    return uk

def CAP(ukm,ek,ekm,Te,a,T):
    A = T/(Te+T)
    B = (Te+a*T)/(Te+T)
    C = a*T/(Te+T)
    uk = A*ukm + B*ek - C*ekm
    return uk

# Question 3: Utilisation

i0 = 0
w0 = 0
u0 = 0
Li = [i0]
Lw = [w0]
Lu = [u0]
Lt = [ti]
Ltu = [ti]

# Boucle de simulation

k = 0
t = ti
u = u0
ek = 0 # Ou Uc
while t < tf:
    i = Li[-1]
    w = Lw[-1]
    V = [i,w]
    k += 1
    t = ti + k*dt
    r = w * Kcapt
    ekm = ek
    ek = Uc - r
    ukm = u
    # u = CP(ek,0.1)
    u = CPI(ukm,ek,ekm,dt,0.4,7.29)
    # u = CAP(ukm,ek,ekm,dt,2.04,0.0038)
    Ltu.append(t)
    Lu.append(u)
    lt,li,lw = MCC(V,t,t+dt,dts)
    Li += li
    Lw += lw
    Lt += lt

f_Affiche(5,Lt,Li,'i')
f_Affiche(5,Lt,Lw,'w')
Affiche_Creneaux(6,Ltu,Lu)