Note
Bonjour et bienvenue dans la communauté SunFounder pour les passionnés de Raspberry Pi, Arduino et ESP32 sur Facebook ! Plongez dans l’univers de Raspberry Pi, Arduino et ESP32 avec d’autres passionnés.
Pourquoi rejoindre ?
Support d’experts : Résolvez vos problèmes après-vente et vos défis techniques grâce à l’aide de notre communauté et de notre équipe.
Apprendre & Partager : Échangez des astuces et des tutoriels pour perfectionner vos compétences.
Aperçus exclusifs : Accédez en avant-première aux annonces de nouveaux produits et aux aperçus exclusifs.
Réductions spéciales : Profitez de réductions exclusives sur nos nouveaux produits.
Promotions festives et concours : Participez à des concours et à des promotions spéciales pendant les fêtes.
👉 Prêt à explorer et créer avec nous ? Cliquez sur [Ici] et rejoignez-nous dès aujourd’hui !
3.1.6 Contrôle de mouvement
Introduction
Dans cette leçon, nous allons créer un dispositif simple de détection et de contrôle de mouvement. Le MPU6050 est utilisé comme capteur et le moteur pas à pas comme dispositif contrôlé. En fixant le MPU6050 sur un gant, vous pouvez contrôler le moteur pas à pas en faisant pivoter votre poignet.
Composants

Schéma de câblage
T-Board Name |
physical |
wiringPi |
BCM |
GPIO18 |
Pin 12 |
1 |
18 |
GPIO23 |
Pin 16 |
4 |
23 |
GPIO24 |
Pin 18 |
5 |
24 |
GPIO25 |
Pin 22 |
6 |
25 |
SDA1 |
Pin 3 |
||
SCL1 |
Pin 5 |

Procédures expérimentales
Étape 1 : Construisez le circuit.

Pour les utilisateurs du langage C
Étape 2 : Accédez au dossier du code.
cd ~/davinci-kit-for-raspberry-pi/c/3.1.6/
Étape 3 : Compilez le code.
gcc 3.1.6_MotionControl.c -lwiringPi -lm
Étape 4 : Exécutez le fichier exécutable.
sudo ./a.out
Lorsque le code s’exécute, si l’angle d’inclinaison du mpu6050 sur l’axe Y-axis dépasse 45 °, le moteur pas à pas tourne dans le sens antihoraire ; s’il est inférieur à -45 °, le moteur tourne dans le sens horaire.
Note
Si cela ne fonctionne pas après l’exécution ou si un message d’erreur apparaît : "wiringPi.h: Aucun fichier ou dossier de ce type », veuillez vous référer à C code is not working?.
Explication du code
double mpu6050(){
acclX = read_word_2c(0x3B);
acclY = read_word_2c(0x3D);
acclZ = read_word_2c(0x3F);
acclX_scaled = acclX / 16384.0;
acclY_scaled = acclY / 16384.0;
acclZ_scaled = acclZ / 16384.0;
double angle=get_y_rotation(acclX_scaled, acclY_scaled, acclZ_scaled);
return angle;
}
La fonction mpu6050 obtient l’angle d’inclinaison dans la direction de l’axe Y.
void rotary(char direction){
if(direction == 'c'){
for(int j=0;j<4;j++){
for(int i=0;i<4;i++)
{digitalWrite(motorPin[i],0x99>>j & (0x08>>i));}
delayMicroseconds(stepSpeed);
}
}
else if(direction =='a'){
for(int j=0;j<4;j++){
for(int i=0;i<4;i++)
{digitalWrite(motorPin[i],0x99<<j & (0x80>>i));}
delayMicroseconds(stepSpeed);
}
}
}
Si la touche de direction reçue est “c”, le moteur pas à pas tourne dans le sens horaire ; si la touche est “a”, le moteur tourne dans le sens antihoraire. Pour plus de détails sur le calcul de la direction de rotation du moteur pas à pas, référez-vous à 1.3.3 Stepper Motor.
int main()
{
setup();
double angle;
while(1) {
angle = mpu6050();
if (angle >=45){rotary('a');}
else if (angle<=-45){rotary('c');}
}
return 0;
}
L’angle d’inclinaison dans la direction de l’axe Y est lu depuis mpu6050, et s’il est supérieur à 45 °, le moteur pas à pas tourne dans le sens antihoraire ; s’il est inférieur à -45 °, le moteur tourne dans le sens horaire.
Pour les utilisateurs du langage Python
Étape 2 : Accédez au dossier du code.
cd ~/davinci-kit-for-raspberry-pi/python/
Étape 3 : Exécutez le fichier exécutable.
sudo python3 3.1.6_MotionControl.py
Lorsque le code s’exécute, si l’angle d’inclinaison du mpu6050 sur l’axe Y - axis dépasse 45 °, le moteur pas à pas tourne dans le sens antihoraire ; s’il est inférieur à -45 °, le moteur tourne dans le sens horaire.
Code
Note
Vous pouvez modifier/réinitialiser/copier/exécuter/arrêter le code ci-dessous. Mais avant cela, vous devez vous rendre sur le chemin du code source comme davinci-kit-for-raspberry-pi/python
.
import RPi.GPIO as GPIO
import smbus
import math
import time
# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c
bus = smbus.SMBus(1)
address = 0x68
bus.write_byte_data(address, power_mgmt_1, 0)
#Stepper motor pins
motorPin = (18,23,24,25)
rolePerMinute =15
stepsPerRevolution = 2048
stepSpeed = (60/rolePerMinute)/stepsPerRevolution
#mpu6050
def read_byte(adr):
return bus.read_byte_data(address, adr)
def read_word(adr):
high = bus.read_byte_data(address, adr)
low = bus.read_byte_data(address, adr+1)
val = (high << 8) + low
return val
def read_word_2c(adr):
val = read_word(adr)
if (val >= 0x8000):
return -((65535 - val) + 1)
else:
return val
def dist(a,b):
return math.sqrt((a*a)+(b*b))
def get_y_rotation(x,y,z):
radians = math.atan2(x, dist(y,z))
return -math.degrees(radians)
def get_x_rotation(x,y,z):
radians = math.atan2(y, dist(x,z))
return math.degrees(radians)
def mpu6050():
accel_xout = read_word_2c(0x3b)
accel_yout = read_word_2c(0x3d)
accel_zout = read_word_2c(0x3f)
accel_xout_scaled = accel_xout / 16384.0
accel_yout_scaled = accel_yout / 16384.0
accel_zout_scaled = accel_zout / 16384.0
angle=get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
return angle
#Stepper Motor
def rotary(direction):
if(direction == 'c'):
for j in range(4):
for i in range(4):
GPIO.output(motorPin[i],0x99>>j & (0x08>>i))
time.sleep(stepSpeed)
elif(direction == 'a'):
for j in range(4):
for i in range(4):
GPIO.output(motorPin[i],0x99<<j & (0x80>>i))
time.sleep(stepSpeed)
def setup():
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
for i in motorPin:
GPIO.setup(i, GPIO.OUT)
def loop():
while True:
angle=mpu6050()
if angle >=45 :
rotary('a')
elif angle <=-45:
rotary('c')
def destroy():
GPIO.cleanup()
if __name__ == '__main__':
setup()
try:
loop()
except KeyboardInterrupt:
destroy()
Explication du code
def mpu6050():
accel_xout = read_word_2c(0x3b)
accel_yout = read_word_2c(0x3d)
accel_zout = read_word_2c(0x3f)
accel_xout_scaled = accel_xout / 16384.0
accel_yout_scaled = accel_yout / 16384.0
accel_zout_scaled = accel_zout / 16384.0
angle=get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
return angle
La fonction mpu6050 obtient l’angle d’inclinaison dans la direction de l’axe Y.
def rotary(direction):
if(direction == 'c'):
for j in range(4):
for i in range(4):
GPIO.output(motorPin[i],0x99>>j & (0x08>>i))
time.sleep(stepSpeed)
elif(direction == 'a'):
for j in range(4):
for i in range(4):
GPIO.output(motorPin[i],0x99<<j & (0x80>>i))
time.sleep(stepSpeed)
Si la touche de direction reçue est “c”, le moteur pas à pas tourne dans le sens horaire ; si la touche est “a”, le moteur tourne dans le sens antihoraire. Pour plus de détails sur le calcul de la direction de rotation du moteur pas à pas, référez-vous à 1.3.3 Stepper Motor.
def loop():
while True:
angle=mpu6050()
if angle >=45 :
rotary('a')
elif angle <=-45:
rotary('c')
L’angle d’inclinaison dans la direction de l”axe Y est lu depuis mpu6050, et s’il est supérieur à 45 °, la fonction rotary() est appelée pour faire tourner le moteur pas à pas dans le sens antihoraire ; s’il est inférieur à -45 °, le moteur tourne dans le sens horaire.
Image du phénomène
