Commit 2b1a0921 authored by behrajor's avatar behrajor

ajout du code python pour le controle du bras

parent 848b74ba
#!/bin/python3.7
# -*-coding:Utf-8 -*
"""
Fonctions permettant de controller le bras robot AL5D
réalisé dans le cadre de la PR "bras robot intelligent" réalisé en P19
calcul du modèle inverse et communication avec la carte Arduino pour envoyer les commandes
"""
from math import *
from time import sleep
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
T = 1
#Erreurs sur les angles et position :
eP = 7
eC = -1
eE = 4
eHC = 4
eR = -30
#Constante de longueur :
L1 = 146
L2 = 185
Zb = 70
def calculAngles(x, y, z):
"""Fonction chagée de calculer le modèle inverse du bras robot
calcul des angles que les différents moteurs du bras doivent prendre en fonction des coordonnée à atteindre
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))
L3 = (pow(L, 2)-pow(L2, 2)+pow(L1, 2))/(2*L)
angle["B"] = int(degrees(atan2(y, x)))
angle["E"] = int(degrees(atan2(z-Zb, r) + acos(L3/L1))) + eE
angle["C"] = int(180 - degrees(asin(L3/L1)) - degrees(asin((L-L3)/L2))) + eC
#On met par defaut la pince à l'horizontal :
angle["P"] = int(angle["C"] - angle["E"] + 90) + eP
def move(x, y, z):
"""Fonction qui envoie une commande de déplacement du bras
Action sur les moteurs B, E, C et P
envoie des informations par USB
"""
calculAngles(x, y, z)
cmd = "P{0}".format(angle["P"])
ser.write(cmd.encode())
print(cmd.encode())
sleep(T)
cmd = "E{0}C{1}".format(angle["E"], angle["C"])
ser.write(cmd.encode())
print(cmd.encode())
sleep(T)
cmd = "B{0}".format(angle["B"])
ser.write(cmd.encode())
print(cmd.encode())
sleep(T)
def moveAngle(B, E, C, P, G):
"""Fonction pour déplacer le bras en envoyant directement les angles à faire.
L'erreur des angles est prise en compte.
"""
angle["E"] = E + eE
angle["C"] = C + eC
angle["B"] = B
angle["P"] = P + eP
angle["G"] = G
cmd = "E{0}C{1}".format(angle["E"], angle["C"])
ser.write(cmd.encode())
print(cmd.encode())
sleep(T)
cmd = "P{0}".format(angle["P"])
ser.write(cmd.encode())
print(cmd.encode())
sleep(T)
cmd = "B{0}".format(angle["B"])
ser.write(cmd.encode())
print(cmd.encode())
sleep(T)
cmd = "E{0}".format(angle["E"])
ser.write(cmd.encode())
print(cmd.encode())
sleep(T)
def move0():
angle["E"] = 120 + eE
angle["C"] = 120 + eC
angle["B"] = 90
angle["P"] = 90 + eP
cmd = "E{0}C{1}".format(angle["E"], angle["C"])
ser.write(cmd.encode())
print(cmd.encode())
sleep(T)
cmd = "P{0}".format(angle["P"])
ser.write(cmd.encode())
print(cmd.encode())
sleep(T)
cmd = "B{0}".format(angle["B"])
ser.write(cmd.encode())
print(cmd.encode())
sleep(T)
def ouvrirPince():
"""Fonction pour ouvrir la pince
"""
angle["G"] = 40
cmd = "G{0}".format(angle["G"])
ser.write(cmd.encode())
sleep(T)
def fermerPince():
"""Fonction pour fermer la pince
"""
angle["G"] = 160
cmd = "G{0}".format(angle["G"])
ser.write(cmd.encode())
sleep(T)
def attraperObjet(x, y):
"""Fonction pour attraper un objet placé au sol, ayant pour coordonnée (x, y)
"""
ouvrirPince()
move(x, y, 80) #On déplace l'articulation de la pince 90 mm au dessus de l'objet à attraper
angle["P"] -= 90 #Passage de horizontal à vertical pour la pince
cmd = "P{0}".format(angle["P"])
ser.write(cmd.encode())
sleep(T)
fermerPince()
move0()
def poserObjet(x, y, z):
"""Fonction pour déposer un objet (pas forcément au sol)
"""
move(x, y, z) #On déplace l'articulation de la pince 90 mm au dessus de l'objet à attraper
angle["P"] -= 90 #Passage de horizontal à vertical pour la pince
cmd = "P{0}".format(angle["P"])
ser.write(cmd.encode())
sleep(T)
ouvrirPince()
move0()
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