Oui en effet, on ne peut pas dire grand chose de plus, désolé…
Voici ci-dessous le branchement (j’ai fait comme j’ai pu) :
Et voici la partie du code, il y a une partie initialisation que je n’ai pas mis car il n’y a aucune erreur provenant de cette partie. De plus j’utilise un filtrage par convolution avec scipy via un filtre RIF afin d’avoir des valeurs assez potables.( Si jamais vous avez d’autres moyens je suis preneur)
import smbus
import math
import time
from mpu6050 import mpu6050
import scipy.signal
#Fonctions
def dist(a,b):
return math.sqrt(a**2 +b**2)
def get_y_angle(x,y,z):
radians=math.atan2(x,dist(y,z))
return -math.degrees(radians)
def get_x_angle(x,y,z):
radians=math.atan2(y,dist(x,z))
return math.degrees(radians)
def get_z_angle(x,y,z):
radians=math.atan2(dist(x,y),z)
return math.degrees(radians)
def filtrage_convolution(x,b):
N=len(b)
ne=len(x)
filtre=0
for k in range(N):
filtre+=b[k]*x[(ne-1)-k]
return filtre
def moyenne(L):
s=0
for elt in L:
s+=elt
return s/(len(L))
#Définitions des pins moteurs
M1_En=21
M1_In1=20
M1_In2=16
M2_En=18
M2_In1=23
M2_In2=24
#Création d'une liste de pins pour chaque moteurs
Pins=[[M1_En, M1_In1,M1_In2],[M2_En, M2_In1,M2_In2]]
#Setup
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(True)
GPIO.setup(M1_En,GPIO.OUT)
GPIO.setup(M1_In1,GPIO.OUT)
GPIO.setup(M1_In2,GPIO.OUT)
GPIO.setup(M2_En,GPIO.OUT)
GPIO.setup(M2_In1,GPIO.OUT)
GPIO.setup(M2_In2,GPIO.OUT)
M1_Vitesse=GPIO.PWM(M1_En,100)
M2_Vitesse=GPIO.PWM(M2_En,100)
M1_Vitesse.start(100)
M2_Vitesse.start(100)
def sens1(moteurNum):
GPIO.output(Pins[moteurNum-1][1],GPIO.HIGH)
GPIO.output(Pins[moteurNum-1][2],GPIO.LOW)
def sens2(moteurNum):
GPIO.output(Pins[moteurNum-1][1],GPIO.LOW)
GPIO.output(Pins[moteurNum-1][2],GPIO.HIGH)
def arret(moteurNum):
GPIO.output(Pins[moteurNum-1][1],GPIO.LOW)
GPIO.output(Pins[moteurNum-1][2],GPIO.LOW)
print("Moteur",moteurNum,"arret.")
def arretComplet():
GPIO.output(Pins[1][1],GPIO.LOW)
GPIO.output(Pins[1][2],GPIO.LOW)
GPIO.output(Pins[0][1],GPIO.LOW)
GPIO.output(Pins[0][2],GPIO.LOW)
print("Moteur arretes.")
#......
#Listes des variables
b1=scipy.signal.firwin(nutmaps=5,cutoff=[0.04],window='han',pass_zero='lowpass',fs=8000)#8000 Hz est la fréquence d'échantillonnage en sortie de l'accéléro
Y=[]
Yfiltre=[]
X=[]
Xfiltre=[]
Z=[]
Zfiltre=[]
RotX=[]
RotXfiltre=[]
RotY=[]
RotYfiltre=[]
ordre=len(b1)
while True :
#filtrage de l'angle donné par les valeurs de translations
accel_data=mpu.get_accel_data()
Ax=accel_data['x']/16384
Ay=accel_data['y']/16384
Az=accel_data['z']/16384
angle_y=get_y_angle(Ax,Ay,Az)/1.3
angle_x=get_x_angle(Ax,Ay,Az)/1.3
angle_z=get_z_angle(Ax,Ay,Az)/1.3
X.append(angle_x)
Xfiltre.append(filtrage_convolution(X,b1))#seul angle qui nous intéresse
print("")
print(Xfiltre[len(Xfiltre)-1])
print("")
if Xfiltre[len(Xfiltre)-1]<X0: #X0 est la valeur trouvée lors de l'initialisation
#cas selon si l'angle est supérieur ou inférieur à X0, ici, l'angle diminue quand le poignet se casse vers l'avant avec l'accéléro face à soit
while abs(Xfiltre[len(Xfiltre)-1]-X0)<1.5: #tolérance de +/- 1.5 degré
sens2(2)#va dans le sens inverse du poignet i.e : la gauche ( vu en face de l'arbre moteur)
accel_data=mpu.get_accel_data()
Ax=accel_data['x']/16384
Ay=accel_data['y']/16384
Az=accel_data['z']/16384
angle_x=get_x_angle(Ax,Ay,Az)/1.3
X.append(angle_x)
Xfiltre.append(filtrage_convolution(X,b1))
print("")
print(Xfiltre[len(Xfiltre)-1])
print("")
time.sleep(0.04)
arret(2)
else:
while abs(Xfiltre[len(Xfiltre)-1]-X0)<1.5:
sens1(2)
accel_data=mpu.get_accel_data()
Ax=accel_data['x']/16384
Ay=accel_data['y']/16384
Az=accel_data['z']/16384
angle_x=get_x_angle(Ax,Ay,Az)/1.3
X.append(angle_x)
Xfiltre.append(filtrage_convolution(X,b1))
print("")
print(Xfiltre[len(Xfiltre)-1])
print("")
time.sleep(0.04)
arret(2)
P.S: Le motoréducteur vient de la marque ICQUANZX
