Commit db8a046e authored by Thomas Muller's avatar Thomas Muller
Browse files

Upload Asservissement.py

parent 4f7429bb
# -*- coding: utf-8 -*-
"""
Created on Sat Apr 24 11:29:26 2021
@author: thomas
"""
# Fonctions pour l'asservissement
### PARAMETRES MODELE ET PERFORMANCES ASSERVISSEMENT
kb = 6.1; # Parametre de la modelisation du systeme
Te = 0.1; # Periode d'echantillonnage
alpha = 0.5; # Double zero polynome C(z) = (z-alpha)**2 (D(z) = C(z)*P(z))
tps_rep = 1.3; # Temps de reponse
tau = tps_rep/5.0; # Constante de temps tau
z1 = np.exp(-Te/tau); # Double zero polynome P(z) = (z-z1)**2 (double pole de la boucle fermee)
### COEFFICIENTS POLYNOMES
K = kb*Te*Te/2.0; # Gain du systeme echantillone
# Coefs polynome S
s0 = 1.0;
s1 = 2.0 - 2.0*(alpha + z1);
s2 = (1.0/4.0)*(z1*z1+4.0*z1*alpha+alpha*alpha-1.0+6.0*(1-alpha-z1)+2.0*z1*alpha*(alpha+z1)+z1*z1*alpha*alpha);
# Coefs polynome R
r0 = (1.0/K)*(-2.0*z1*alpha*(alpha+z1)-2.0*(1-alpha-z1)+3.0*s2-z1*z1*alpha*alpha);
r1 = (1.0/K)*(z1*z1*alpha*alpha-s2);
Q = (r0 + r1)/((1-alpha)*(1-alpha));
### FONCTIONS DE CALCUL DES ECHANTILLONS TEMPORELS
def u(k, simu, saturation):
commande = ((-s1*simu[1,k-1] - s2*simu[1,k-2] + Q*simu[0,k] - 2*alpha*Q*simu[0,k-1] + Q*alpha*alpha*simu[0,k-2] - r0*simu[2,k-1] - r1*simu[2,k-2])/s0)
if(commande > saturation):
return saturation
elif (commande < -saturation):
return -saturation
else:
return commande
def y1(k, simu):
return (2*z1*simu[2,k-1] - z1*z1*simu[2,k-2] + Q*K*(simu[0,k-1] + simu[0,k-2]))
def y2(k, simu):
return (2*simu[2,k-1] - simu[2,k-2] + K*(simu[1,k-1] + simu[1,k-2]))
def swap(simu):
simu[0,0] = simu[0,1]
simu[0,1] = simu[0,2]
simu[1,0] = simu[1,1]
simu[1,1] = simu[1,2]
simu[2,0] = simu[2,1]
simu[2,1] = simu[2,2]
return;
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment