Commit b5cbbc7e authored by behrajor's avatar behrajor

dernière mise à jour pour le controle du bras

parent b1421e30
......@@ -12,23 +12,23 @@ import serial
#/connaitre le port à utiliser : python -m serial.tools.list_ports
ser = serial.Serial('/dev/ttyACM0',9600) #Création d'un objet serial
angle = {"B":90, "E":120, "C":120, "P":90, "G":40} #anlges initiaux
cmd = str()
#temp de repos
cmd = str() #Commande envoyée à l'arduino (variable)
#temp de repos entre chaque mouvement
T = 1
#Erreurs sur les angles et position :
#Erreurs sur les angles :
eP = 7
eC = -1
eE = 4
eHC = 4
#coefficients de l'erreur vairable (considéré linéaire) :
#coefficients de l'erreur vairable :
er = 30
eAz = -2.4E-3
eBz = 0.581
eCz = -20.7
#Constante de longueur :
#Constantes de longueur :
L1 = 146
L2 = 185
Zb = 70
......@@ -43,10 +43,7 @@ def calculAngles(x, y, z):
r = sqrt(pow(x, 2)+pow(y, 2))
#En prenant en compte l'erreur proportionnelle :
r -= er
print("er : {}".format(er))
print("r : {}".format(r))
z = z - eAz * pow(r, 2) - eBz * r - eCz
print("z : {}".format(z))
L = sqrt(pow(r, 2)+pow(z, 2))
L3 = (pow(L, 2)-pow(L2, 2)+pow(L1, 2))/(2*L)
......
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