Nota

¡Hola, bienvenidos a la Comunidad de Entusiastas de SunFounder para Raspberry Pi, Arduino y ESP32 en Facebook! Profundiza en Raspberry Pi, Arduino y ESP32 junto a otros entusiastas.

¿Por qué unirse?

Soporte Experto: Resuelve problemas postventa y desafíos técnicos con la ayuda de nuestra comunidad y equipo.
Aprender y Compartir: Intercambia consejos y tutoriales para mejorar tus habilidades.
Avances Exclusivos: Accede anticipadamente a anuncios de nuevos productos y adelantos exclusivos.
Descuentos Especiales: Disfruta de descuentos exclusivos en nuestros productos más recientes.
Promociones y Sorteos Festivos: 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, haremos un dispositivo sencillo de detección y control de movimiento. El MPU6050 se utiliza como sensor y el motor paso a paso como dispositivo controlado. Con el MPU6050 montado en un guante, puedes controlar el motor girando la muñeca.

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: Arma el circuito.

Para Usuarios de Lenguaje C

Paso 2: Ve a la carpeta del código.

cd ~/davinci-kit-for-raspberry-pi/c/3.1.6/

Paso 3: Compila el código.

gcc 3.1.6_MotionControl.c -lwiringPi -lm

Paso 4: Ejecuta el archivo compilado.

sudo ./a.out

Mientras el código se ejecuta, si el ángulo de inclinación del mpu6050 en el eje Y axis es mayor a 45 °C, el motor paso a paso gira en sentido antihorario; si es menor a -45 °C, el motor gira en sentido horario.

Nota

Si no funciona después de ejecutarlo, o aparece un mensaje de error: "wiringPi.h: No such file or directory», consulta ¿El código en C no funciona?.

Explicación del Código

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;
}

mpu6050 obtiene el ángulo de inclinación en la dirección del eje 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 dirección key recibida es 'c', el motor paso a paso gira en sentido horario; si la key es 'a', el motor gira en sentido antihorario. Consulta 1.3.3 Stepper Motor para más detalles sobre el cálculo de la dirección de rotación del motor.

int main()
{
    setup();
    double angle;
    while(1) {
        angle = mpu6050();
        if (angle >=45){rotary('a');}
        else if (angle<=-45){rotary('c');}
    }
    return 0;
}

El ángulo de inclinación en la dirección del eje Y se lee desde el mpu6050, y si es mayor a 45 °C, el motor paso a paso gira en sentido antihorario; si es menor a -45 °C, el motor gira en sentido horario.

Para Usuarios de Python

Paso 2: Ve a la carpeta del código.

cd ~/davinci-kit-for-raspberry-pi/python/

Paso 3: Ejecuta el archivo.

sudo python3 3.1.6_MotionControl.py

Mientras se ejecuta el código, si el ángulo de inclinación del mpu6050 en el Y axis es mayor a 45 °C, el motor paso a paso gira en sentido antihorario; si es menor a -45 °C, el motor gira en sentido horario.

Código

Nota

Puedes Modificar/Restablecer/Copiar/Ejecutar/Detener el código a continuación. Pero antes, debes dirigirte a la ruta de código fuente como davinci-kit-for-raspberry-pi/python.

import RPi.GPIO as GPIO
import smbus
import math
import time

# Registros de gestión de energía
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

bus = smbus.SMBus(1)
address = 0x68
bus.write_byte_data(address, power_mgmt_1, 0)

# Pines del motor paso a paso
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

# Motor Paso a Paso
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()

Explicación del Código

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

mpu6050 obtiene el ángulo de inclinación en la dirección del eje 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 dirección key recibida es 'c', el motor paso a paso gira en sentido horario; si la key es 'a', el motor gira en sentido antihorario. Consulta 1.3.3 Stepper Motor para más detalles sobre el cálculo de la dirección de rotación del motor.

def loop():
    while True:
        angle=mpu6050()
        if angle >=45 :
            rotary('a')
        elif angle <=-45:
            rotary('c')

El ángulo de inclinación en la dirección del eje Y se lee desde mpu6050. Si es mayor a 45 °C, se llama a rotary() para que el motor paso a paso gire en sentido antihorario; si es menor a -45 °C, el motor gira en sentido horario.

3.1.6 Control de Movimiento

Introducción

Componentes

Diagrama Esquemático

Procedimientos Experimentales

Para Usuarios de Lenguaje C

Para Usuarios de Python

Imagen del Fenómeno