Nota
¡Hola! Bienvenido a la comunidad de entusiastas de SunFounder Raspberry Pi, Arduino y ESP32 en Facebook. Únete a otros entusiastas y profundiza en el mundo de Raspberry Pi, Arduino y ESP32.
¿Por qué unirse?
Soporte de expertos: Resuelve problemas postventa y desafíos técnicos con la ayuda de nuestra comunidad y equipo.
Aprende y comparte: Intercambia consejos y tutoriales para mejorar tus habilidades.
Vistas previas exclusivas: Obtén acceso anticipado a anuncios de nuevos productos y adelantos.
Descuentos especiales: Disfruta de descuentos exclusivos en nuestros productos más recientes.
Promociones festivas y sorteos: Participa en sorteos y promociones de temporada.
👉 ¿Listo para explorar y crear con nosotros? Haz clic en [Aquí] y únete hoy mismo.
3.1.6 Control de Movimiento
Introducción
En esta lección, crearemos un dispositivo simple de detección y control de movimiento. Utilizaremos el MPU6050 como sensor y un motor paso a paso como dispositivo controlado. Al montar el MPU6050 en un guante, podrás controlar el motor paso a paso rotando tu muñeca.
Componentes Necesarios
En este proyecto, necesitaremos los siguientes componentes.
Diagrama Esquemático
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 |
Procedimientos Experimentales
Paso 1: Construir el circuito.
Paso 2: Abre el archivo de código.
cd ~/davinci-kit-for-raspberry-pi/python-pi5
Paso 3: Ejecuta el código.
sudo python3 3.1.6_MotionControl.py
Al ejecutar el código, si el ángulo de inclinación de mpu6050 en el eje Y-axis es mayor a 45°, el motor paso a paso girará en sentido antihorario; si es menor a -45°, el motor girará en sentido horario.
Advertencia
Si aparece el error RuntimeError: Cannot determine SOC peripheral base address, consulta Si gpiozero no funciona.
Código
Nota
Puedes Modificar/Restablecer/Copiar/Ejecutar/Detener el código a continuación. Antes de eso, debes acceder a la ruta del código fuente como davinci-kit-for-raspberry-pi/python-pi5. Después de modificar el código, puedes ejecutarlo directamente para ver el efecto.
#!/usr/bin/env python3
from gpiozero import OutputDevice
import smbus
import math
import time
# Inicializa los registros de gestión de energía para el MPU6050
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c
# Configura la comunicación I2C con el MPU6050
bus = smbus.SMBus(1) # Inicializa SMBus
address = 0x68 # Dirección I2C de MPU6050
bus.write_byte_data(address, power_mgmt_1, 0) # Activa el MPU6050
# Inicializa los pines del motor a GPIO 18, 23, 24, 25
motorPin = [OutputDevice(pin) for pin in (18, 23, 24, 25)]
# Define parámetros de velocidad de rotación del motor
rolePerMinute = 15
stepsPerRevolution = 2048
# Calcula el retraso entre pasos para la RPM deseada
stepSpeed = (60 / rolePerMinute) / stepsPerRevolution
# Lee un solo byte desde la dirección I2C especificada
def read_byte(adr):
return bus.read_byte_data(address, adr)
# Lee una palabra (2 bytes) desde la dirección I2C especificada
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
# Lee una palabra en formato de complemento a 2
def read_word_2c(adr):
val = read_word(adr)
if val >= 0x8000:
return -((65535 - val) + 1)
else:
return val
# Calcula la distancia euclidiana entre dos puntos
def dist(a, b):
return math.sqrt((a * a) + (b * b))
# Calcula la rotación en el eje Y
def get_y_rotation(x, y, z):
radians = math.atan2(x, dist(y, z))
return -math.degrees(radians)
# Calcula la rotación en el eje X
def get_x_rotation(x, y, z):
radians = math.atan2(y, dist(x, z))
return math.degrees(radians)
# Obtiene el ángulo de inclinación del MPU6050
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
# Controla la rotación del motor paso a paso
def rotary(direction):
if direction == 'c':
# Secuencia de rotación en sentido horario
for j in range(4):
for i in range(4):
if 0x99 >> j & (0x08 >> i):
motorPin[i].on()
else:
motorPin[i].off()
time.sleep(stepSpeed)
elif direction == 'a':
# Secuencia de rotación en sentido antihorario
for j in range(4):
for i in range(4):
if 0x99 << j & (0x08 >> i):
motorPin[i].on()
else:
motorPin[i].off()
time.sleep(stepSpeed)
# Bucle principal para leer continuamente el ángulo de inclinación y controlar el motor
try:
while True:
angle = mpu6050()
if angle >= 45:
rotary('a') # Gira en sentido antihorario para inclinación positiva
elif angle <= -45:
rotary('c') # Gira en sentido horario para inclinación negativa
except KeyboardInterrupt:
# Apaga todos los pines del motor al interrumpir con teclado
for pin in motorPin:
pin.off()
Explicación del Código
El script comienza importando las bibliotecas necesarias.
gpiozeropara controlar los pines GPIO,smbuspara la comunicación I2C,mathpara operaciones matemáticas ytimepara introducir retardos.#!/usr/bin/env python3 from gpiozero import OutputDevice import smbus import math import time
Configura la comunicación I2C con el sensor MPU6050.
power_mgmt_1ypower_mgmt_2son registros para gestionar la energía del sensor. El sensor se «despierta» escribiendo enpower_mgmt_1.# Inicializa los registros de gestión de energía para el MPU6050 power_mgmt_1 = 0x6b power_mgmt_2 = 0x6c # Configura la comunicación I2C con el MPU6050 bus = smbus.SMBus(1) # Inicializa SMBus address = 0x68 # Dirección I2C del MPU6050 bus.write_byte_data(address, power_mgmt_1, 0) # Activa el MPU6050
Inicializa los pines GPIO (18, 23, 24, 25) en la Raspberry Pi para controlar el motor paso a paso. Cada pin está asociado con una bobina en el motor.
# Inicializa los pines del motor a GPIO 18, 23, 24, 25 motorPin = [OutputDevice(pin) for pin in (18, 23, 24, 25)]
Define las revoluciones por minuto (RPM) del motor y el número de pasos por revolución.
stepSpeedcalcula el retraso entre pasos para alcanzar las RPM deseadas, asegurando un funcionamiento suave del motor.# Define parámetros de velocidad de rotación del motor rolePerMinute = 15 stepsPerRevolution = 2048 # Calcula el retraso entre pasos para la RPM deseada stepSpeed = (60 / rolePerMinute) / stepsPerRevolution
Estas funciones se utilizan para la comunicación I2C.
read_bytelee un solo byte de una dirección dada, mientras queread_wordlee dos bytes (una palabra) combinándolos en un solo valor mediante operaciones de bits (<<y+).# Lee un byte desde la dirección I2C especificada def read_byte(adr): return bus.read_byte_data(address, adr) # Lee una palabra (2 bytes) desde la dirección I2C especificada 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
Esta función convierte la palabra leída en formato de complemento a 2, lo cual es útil para interpretar valores con signo de los datos del sensor. Esta conversión es necesaria para manejar lecturas negativas.
# Lee una palabra en formato de complemento a 2 def read_word_2c(adr): val = read_word(adr) if val >= 0x8000: return -((65535 - val) + 1) else: return val
distcalcula la distancia euclidiana entre dos puntos, utilizada en los cálculos de rotación.get_y_rotationyget_x_rotationcalculan los ángulos de rotación a lo largo de los ejes Y y X, respectivamente, usando la funciónatan2de la bibliotecamathy convirtiendo el resultado a grados.# Calcula la distancia euclidiana entre dos puntos def dist(a, b): return math.sqrt((a * a) + (b * b)) # Calcula la rotación en el eje Y def get_y_rotation(x, y, z): radians = math.atan2(x, dist(y, z)) return -math.degrees(radians) # Calcula la rotación en el eje X def get_x_rotation(x, y, z): radians = math.atan2(y, dist(x, z)) return math.degrees(radians)
Esta función lee los datos del acelerómetro del sensor MPU6050, escala las lecturas y calcula el ángulo de inclinación usando la función
get_y_rotation. La funciónread_word_2clee datos del sensor en formato de complemento a 2 para manejar valores negativos.# Obtiene el ángulo de inclinación del MPU6050 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 función
rotarycontrola la rotación del motor paso a paso. Ejecuta una secuencia de pasos para la rotación en sentido horario o antihorario, según el parámetrodirection. La secuencia involucra activar o desactivar pines específicos del motor en un patrón.# Controla la rotación del motor paso a paso def rotary(direction): if direction == 'c': # Secuencia de rotación en sentido horario for j in range(4): for i in range(4): if 0x99 >> j & (0x08 >> i): motorPin[i].on() else: motorPin[i].off() time.sleep(stepSpeed) elif direction == 'a': # Secuencia de rotación en sentido antihorario for j in range(4): for i in range(4): if 0x99 << j & (0x08 >> i): motorPin[i].on() else: motorPin[i].off() time.sleep(stepSpeed)
El bucle principal lee continuamente el ángulo de inclinación del sensor MPU6050 y controla la dirección de rotación del motor según el ángulo. Si el programa se interrumpe (por ejemplo, mediante una interrupción de teclado), apaga todos los pines del motor por seguridad.
# Bucle principal para leer continuamente el ángulo de inclinación y controlar el motor try: while True: angle = mpu6050() if angle >= 45: rotary('a') # Gira en sentido antihorario para inclinación positiva elif angle <= -45: rotary('c') # Gira en sentido horario para inclinación negativa except KeyboardInterrupt: # Apaga todos los pines del motor al interrumpir con el teclado for pin in motorPin: pin.off()