.. note::

    ¡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 [|link_sf_facebook|] 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
--------------

.. image:: img/list_Motion_Control.png
    :align: center

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             
============ ======== ======== ===

.. image:: img/Schematic_three_one6.png
   :align: center

Procedimientos Experimentales
--------------------------------

**Paso 1:** Arma el circuito.

.. image:: img/image251.png
   :alt: 3.1.6 Motion Control_bb
   :width: 800
   :align: center

**Para Usuarios de Lenguaje C**
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

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

.. raw:: html

   <run></run>

.. code-block:: 

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

**Paso 3**: Compila el código.

.. raw:: html

   <run></run>

.. code-block:: 

    gcc 3.1.6_MotionControl.c -lwiringPi -lm

**Paso 4**: Ejecuta el archivo compilado.

.. raw:: html

   <run></run>

.. code-block:: 

    sudo ./a.out

Mientras el código se ejecuta, si el ángulo de inclinación del **mpu6050** en el eje `Y <https://cn.bing.com/dict/search?q=Y&FORM=BDVSP6&mkt=zh-cn>`_ `axis <https://cn.bing.com/dict/search?q=axis&FORM=BDVSP6&mkt=zh-cn>`_ 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.

.. note::

    Si no funciona después de ejecutarlo, o aparece un mensaje de error: \"wiringPi.h: No such file or directory", consulta :ref:`faq_c_nowork`.

**Explicación del Código**

.. code-block:: c

    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.

.. code-block:: c

    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 :ref:`1.3.3_stepper` para más detalles 
sobre el cálculo de la dirección de rotación del motor.

.. code-block:: c

    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.

.. raw:: html

   <run></run>

.. code-block::

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

**Paso 3**: Ejecuta el archivo.

.. raw:: html

   <run></run>

.. code-block::

    sudo python3 3.1.6_MotionControl.py

Mientras se ejecuta el código, si el ángulo de inclinación del **mpu6050** en el `Y <https://cn.bing.com/dict/search?q=Y&FORM=BDVSP6&mkt=zh-cn>`_ `axis <https://cn.bing.com/dict/search?q=axis&FORM=BDVSP6&mkt=zh-cn>`_ 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**

.. note::

    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``. 
    
.. raw:: html

    <run></run>

.. code-block:: 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**

.. code-block:: python

    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.

.. code-block:: python

    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 :ref:`1.3.3_stepper` para más detalles sobre el cálculo de 
la dirección de rotación del motor.

.. code-block:: python

    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.

Imagen del Fenómeno
-----------------------

.. image:: img/image252.jpeg
   :align: center