Algorithme d'asservissement en position moteur

Bonjour,
Je suis une élève de prépa PSI (physiques sciences industrielles) et je travaille sur un projet d'étude qui porte sur une prothèse de genou pour laquelle on a besoin de détérminer la loi de commande du moteur pour qu'elle puisse fonctionner de façon optiamale. Pour ceci notre prof nous a fabriqué une maquette sur laquelle on travaille et il nous a écrit quelques programmes en MicroPython pour pouvoir commander le moteur et tout. Donc comme je ne connais pas de façon précise la syntaxe de MicroPython je galère un peu, je sais ce qu'il faut qu'il se passe mais je ne sais pas l'écrire ! je vous transmets l'algo sur lequel je travaille pour plus de détails. Mercii
"""
[Email visible pour les membres Pro]: Francois WEISS - 13/02/2017

ce programme asservit la position du rotor moteur à partir d'une comparaison entre "position_potentio" et "compteur_position"
il utilise les voies SA et SB de l'encodeur (relayées par la carte de puissance PmodHB5)
pour déterminer la position du rotor moteur par comptage déclenché par interruptions
sur les voies SA et SB ;
pour SA : si SB est à "1" : incrémentation d'une unité
et si SB est à "0" : décrémentation d'une unité
pour SB : si SA est à "0" : incrémentation d'une unité
et si SA est à "1" : décrémentation d'une unité
de plus, pour information, on allume des leds sur la carte Pyboard pour visualiser le sens moteur et le passage à zéro de l'encodeur ;
- led verte pour moteur commandé en sens "+"
- led jaune pour moteur commandé en sens "-"
- led bleue allumée en position initiale "0" du codeur (position très fugitive).

L'asservissement de position fait appel à une correction proportionnelle-intégrale ; la correction dérivée n'est pas programmée.
Les coefficients du correcteur sont ajustés expérimentalement pour le motoréducteur PMT1-19 et la carte PMOD-HB5

!!!! attention, ce programme gère mal le démarrage, et le rotor moteur peut tourner un certain temps avant de rejoindre une position stable du fait de la présence de l'intégrateur !!!!
prévoir éventuellement une mise en place progressive de celui-ci.
"""

import pyb
import time
# coefficients pour le PID
Kp = 2 # pour le P du PID position
Ki = 10 # pour le I du PID position
Kd = 0 # pour le D du PID position
Te = 0.002 # période d'échantillonnage en seconde
integral_max = 500000 # plafonnnement de l'intégrateur
integrateur_precedent = 0 # variable globale pour le calcul d'intégration dans la fonction PID

# déclare les broches
SA = pyb.Pin.board.Y1 # voie SA branchée sur la broche Y1 de PyBoard
SB = pyb.Pin.board.Y2 # voie SB branchée sur la broche Y2 de PyBoard

#déclare la broche du sens moteur sur Y4 en mode PP(?)
DIR_moteur = pyb.Pin("Y4", pyb.Pin.OUT_PP)
DIR_moteur.low() # place la sortie à 0

# déclare la broche du PWM moteur sur Y3
p=pyb.Pin("Y3") # Y3 has TIM4, CH3
tim=pyb.Timer(4, freq=10000) # le sifflement du PWM devient inaudible au dessus de 10000 Hz
ch=tim.channel(3, pyb.Timer.PWM, pin=p)

# déclare le potentiomètre sur la broche X8
potentio = pyb.ADC("X8") # conversion analogique numérique sur 12 bits = 4096 valeurs

# variable pour l'indication de la position
compteur_position = 0

# écriture de la fonction déclenchée par l'interruption sur la voie SA :
def fonction_interruptionSA(irq1):
global compteur_position
if SB.value() == 1:
compteur_position += 1
if SB.value() == 0:
compteur_position -= 1

# écriture de la fonction déclenchée par l'interruption sur la voie SB :
def fonction_interruptionSB(irq2):
global compteur_position
if SA.value() == 0:
compteur_position += 1
if SA.value() == 1:
compteur_position -= 1

# déclaration de l'interruption SA (sur front montant, sans PULL_UP interne):
irq1 = pyb.ExtInt(SA, pyb.ExtInt.IRQ_RISING, pyb.Pin.PULL_NONE, fonction_interruptionSA)

# déclaration de l'interruption SB (sur front montant, sans PULL_UP interne):
irq2 = pyb.ExtInt(SB, pyb.ExtInt.IRQ_RISING, pyb.Pin.PULL_NONE, fonction_interruptionSB)

def PID(entree) : #seul le PI est programmé
global integrateur_precedent
integrateur = integrateur_precedent + Ki*Te*entree
if integrateur > integral_max:
integrateur = integral_max
integrateur_precedent = integrateur
#derivateur = #non programmé
sortie = Kp*entree
sortie += integrateur
#sortie += derivateur
return sortie

drapeau_periode = 0
def echantillonne(timer):
global drapeau_periode
drapeau_periode = 1
timer = pyb.Timer(1)
timer.init(freq=1/Te, callback=echantillonne)

temps_depart_calcul = pyb.micros() # déclenchement du temps pour la mesure de période d'échantillonnage
while "true" : #la mesure de position "compteur_position" est faite en temps masqué par les interruptions
k = 0
temps_de_calcul = pyb.micros() - temps_depart_calcul
while drapeau_periode == 0 :
#delta_t = time.time() - temps_depart
#time.sleep(0.001) #attente 1 milliseconde
k += 1
if k == 1 :
print(temps_de_calcul)
#print(pyb.micros()-temps_depart)
#attente fin de période d'échantillonnage
drapeau_periode = 0
temps_depart_calcul = pyb.micros() # déclenchement du temps pour la période d'échantillonnage suivante
position_potentio = potentio.read()
ecart = (float(position_potentio) - 2047.0)/5 - float(compteur_position) # /5 pour réduire l'amplitude
commande = int(PID(ecart)) # utilisation de la fonction PID

if commande < 0 :
if commande < -100 : # limitée à 100 car c'est un pourcentage
#de la tension maxi générée par la carte PmodHB5
commande = -100
DIR_moteur.low() # place la sortie à 0 pour sens -
pyb.LED(2).off()
pyb.LED(3).on() #jaune pour visu commande en sens -
else :
if commande >100 : # c'est un pourcentage de la tension maxi générée par la carte PmodHB5
commande = 100
DIR_moteur.high() # place la sortie à 1 pour sens +
pyb.LED(2).on() #vert pour visu commande en sens +
pyb.LED(3).off()
#commande du moteur :
ch.pulse_width_percent(abs(commande))
print("commande : ",commande) #pour debug
print("ecart : ",int(ecart))
# le temps de calcul mesuré est de 200 microsecondes sans les "print" ci-dessus
# il passe à 279 microsecondes si on active les deux "print"