Commit e20b7a0e authored by behrajor's avatar behrajor

ajout de l'erreur proportionnelle sur la position

parent 2b1a0921
......@@ -23,6 +23,11 @@ eC = -1
eE = 4
eHC = 4
eR = -30
#coefficients de l'erreur vairable (considéré linéaire) :
eAr = -0.049
eBr = 34.9
eAz = -0.136
eBz = 21.6
#Constante de longueur :
L1 = 146
......@@ -36,8 +41,12 @@ def calculAngles(x, y, z):
les angles sont stockés dans un dictionnaire avec les nom des moteurs (B, E, C, P et G) comme clé
"""
L = sqrt(pow(x, 2)+pow(y, 2)+pow(z, 2))
r = sqrt(pow(x, 2)+pow(y, 2))
#En prenant en compte l'erreur proportionnelle :
r = (r - eBr)/(1-eAr)
z = z + eAz * r - eBz
L = sqrt(pow(x, 2)+pow(y, 2)+pow(z, 2))
L3 = (pow(L, 2)-pow(L2, 2)+pow(L1, 2))/(2*L)
......@@ -95,6 +104,9 @@ def moveAngle(B, E, C, P, G):
def move0():
"""Retour à une position de référence
position initiale lors de la mise sous tension du bras
"""
angle["E"] = 120 + eE
angle["C"] = 120 + eC
angle["B"] = 90
......
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