''' 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
R = 0.45
L = 0.0046
ke = 0.169
kc = 0.17
f = 0.01
J = 0.01
cr = 0
i0 = 0
w0 = 0
V0 = [i0,w0]
t0 = 0
t1 = 1
N = 1000
u = 10
dt = (t1-t0)/(N-1)

def Euler_Explicite(F,y0,t0,t1,dt):
    W=[0 for k in range(N)]
    I=[0 for k in range(N)]
    T=[k*dt for k in range(N)]
    for k in range(N-1):
        I[k+1]=I[k]+dt*(u-I[k]*R-ke*W[k])/L
        W[k+1]=W[k]+dt*(kc*I[k]-f*W[k])/J
    return T,W,I

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


Lt,Lw,Li = Euler_Explicite(F,V0,t0,t1,dt)

# 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')


#%%
import numpy as np
import matplotlib.pyplot as plt

def F(Y,t): #Y[i,w]
    return np.array([(u-Y[0]*R-ke*Y[1])/L,(kc*Y[0]-f*Y[1])/J])

Y0=np.array([0,0])

from scipy.integrate import odeint
sol=odeint(F,Y0,Lt)

plt.plot(Lt,sol[:,0],color='g')
plt.plot(Lt,sol[:,1],'b')
plt.show()

# 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)