Control

Proyecto Final – Rueda de reacción

Universidad Autónoma de Querétaro
Facultad de Ingeniería
Presentan: Ing. Abigail Gervasio Pérez ; Ing. Angel Yair Alcantara Romero

Resumen— El presente proyecto aborda el diseño, modelado e prácticos del control en la vida real, fortaleciendo habilidades
implementación de un sistema de control PID aplicado a una de diseño, implementación y análisis de sistemas dinámicos.
rueda de reacción capaz de estabilizar la orientación de una Además, la experimentación directa con una rueda de reacción
plataforma semejante a un péndulo invertido. Se desarrolló un
facilita la conexión entre modelos matemáticos y resultados
modelo linealizado de la planta para analizar su comportamiento
experimentales, promoviendo un aprendizaje integral y
dinámico, obteniendo una función de transferencia. A partir de
este análisis, y complementándolo con datos experimentales aplicado.
obtenidos mediante un sensor inercial MPU-6050 y un motor DC
El trabajo comprende el modelado del sistema, la
con encoder, se implementó un controlador PID en una
identificación de sus parámetros dinámicos y la
Raspberry Pi Pico. La sintonización se realizó mediante una
implementación de estrategias de control que permitan regular
combinación del método de Ziegler–Nichols y ajustes empíricos,
logrando un desempeño estable ante perturbaciones. Los
la posición angular del prototipo mediante el principio de
resultados experimentales mostraron respuestas PWM estables,
conservación del momento angular. Asimismo, se evaluó el
control efectivo del motor y capacidad del sistema para mantener comportamiento del sistema bajo diferentes condiciones de
la orientación deseada, validando la viabilidad del enfoque y la operación, con el fin de comprobar la estabilidad, la respuesta
fidelidad del modelo dinámico empleado. transitoria y la robustez del controlador diseñado. Este
proyecto no solo permitió aplicar los conceptos vistos en la
Palabras clave: Péndulo invertido, rueda de reacción, asignatura, sino también comprender los desafíos asociados al
controlador PID, función de transferencia, Ziegler–Nichols, control de sistemas reales y la importancia de una correcta
estabilidad. integración entre hardware, electrónica y algoritmos de
control.

I. INTRODUCCIÓN
II. ANTECEDENTES TEÓRICOS

E
Para el mejor entendimiento sobre lo que se realizó en esta
l estudio de los sistemas de control juega un papel práctica, se mencionan algunos conceptos o palabras clave
fundamental en el diseño y operación de dispositivos utilizadas:
mecatrónicos capaces de regular su comportamiento
ante perturbaciones externas. Entre estos sistemas, las • Planta: Es el sistema o conjunto de actuadores
ruedas de reacción representan una aplicación relevante para controlados por un sistema de control. El controlador se
el control de actitud en plataformas donde no es posible encarga de llevar desde el estado actual del sistema hasta
el punto de referencia.
utilizar actuadores convencionales, como en pequeños
satélites, robots móviles o prototipos de investigación. En este Para el caso de controladores que no reutilizan la
proyecto final se desarrolló un sistema de control basado en información de salida de la planta se denominan sistemas
una rueda de reacción con el propósito de analizar su de lazo abierto. Cuando existe una retroalimentación
dinámica, diseñar un controlador adecuado y validar utilizando la información de salida, entonces estos
experimentalmente su desempeño. sistemas se conocen como de lazo cerrado. [1]

Las ruedas de reacción constituyen una tecnología

ampliamente utilizada en aplicaciones donde se requiere un
control preciso de orientación sin depender de choques
mecánicos o interacción con el ambiente externo. Su estudio
es especialmente relevante en áreas como la robótica, los
sistemas aeroespaciales y los mecanismos de estabilización de Figura 1. Sistema de control de lazo cerrado. [1]
precisión. Realizar este proyecto permite al estudiante
comprender tanto los principios teóricos como los desafíos

1
 Control

• Función de transferencia: Una función de Este vector es perpendicular al plano formado por 𝐫 y
transferencia es una representación matemática que 𝐯, y su dirección se determina mediante la regla de la
describe la relación entre la entrada y la salida de un mano derecha. El momento angular describe la
sistema dinámico, en términos de sus variables de tendencia de una partícula o sistema a continuar en
Laplace (generalmente usando la transformada de movimiento rotacional y constituye la base para el
Laplace). Esta función se usa principalmente para análisis dinámico de cuerpos rígidos y dispositivos
como las ruedas de reacción [5].
sistemas lineales e invariantes en el tiempo (LTI, por
sus siglas en inglés: Linear Time-Invariant) [2].

• Conservación del Momento Angular: El principio de

conservación del momento angular establece que, en
ausencia de torques externos, el momento angular total
de un sistema permanece constante. Esto implica que
Figura 2. Estructura de función de transferencia [2].
cualquier variación interna en la distribución de masas
• Respuesta temporal del sistema: El tiempo es la o en las velocidades angulares debe estar acompañada
variable independiente empleada en la mayoría de los por cambios compensatorios para mantener constante
el momento angular del sistema [6].
sistemas de control, ya que permite analizar la
evolución del estado y salida a lo largo el tiempo. El
desempeño del sistema se evalúa al estudiar su • Rueda de Reacción: Una rueda de reacción es un
comportamiento en el dominio temporal, considerando actuador de control de actitud que consiste en un rotor
la relación entre la entrada y la salida. que gira a altas velocidades y cuyo eje de giro está
La respuesta temporal de un sistema se divide en dos fijado rígidamente a la estructura del vehículo. El
partes: la respuesta transitoria y la respuesta de estado control se obtiene incrementando o disminuyendo la
estable. La respuesta transitoria corresponde a la parte velocidad angular del rotor, lo que genera un torque de
que tiende a desaparecer conforme avanza el tiempo, reacción sobre el eje debido al principio de
mientras que la respuesta e un estado estable es la conservación del momento angular. Las ruedas de
porción que permanece una vez que la transitoria reacción se utilizan comúnmente para controlar la
desaparece. Esta respuesta puede variar en un patrón orientación de satélites estabilizados en tres ejes y
establecido, como onda senoidal, o como función vehículos espaciales pequeños. Son los actuadores de
rampa que incrementa con el tiempo. [3] intercambio de momento más simples y económicos
[7].
• Controlador PID: Un controlador PID (Proporcional–
Integral–Derivativo) es un algoritmo de control en lazo • Sensor MPU-6050: El MPU-6050 es un sensor MEMS
cerrado que calcula la señal de control a partir de la que integra en un solo chip un acelerómetro de tres ejes
combinación lineal de tres términos: el error y un giroscopio de tres ejes. Permite medir
instantáneo (acción proporcional), la suma acumulada aceleraciones lineales y velocidades angulares, siendo
del error (acción integral) y la tasa de cambio del error ampliamente utilizado en sistemas de estabilización,
(acción derivativa). Su objetivo es minimizar la robótica y plataformas de control, debido a su alta
diferencia entre la salida del sistema y la referencia, sensibilidad y facilidad de comunicación mediante I2C
mejorando la estabilidad, el tiempo de respuesta y [8].
reduciendo el error en estado estable [4].
• Motor DC: Un motor de corriente continua (DC)
convierte energía eléctrica en energía mecánica por
medio de la interacción entre un campo magnético y
una corriente eléctrica. Su velocidad se controla
variando el voltaje o utilizando modulación por ancho
de pulso (PWM). Es común en sistemas mecatrónicos
debido a su respuesta rápida y facilidad de control [9].
Figura 3. Controlador PID de una planta [4].
• Encoder: Un encoder es un sensor rotacional que
• Momento Angular: El momento angular de una convierte el movimiento angular en señales digitales
partícula respecto a un origen 𝑂 se define como el capaces de indicar la velocidad y posición del eje.
producto cruz entre el vector posición r y el momento Existen encoders incrementales y absolutos; los
lineal m𝐯 de la partícula: incrementales generan pulsos proporcionales al giro y
permiten estimar la velocidad y dirección. Son
𝐇𝑂 = 𝐫 × 𝑚𝐯

2
 Control

esenciales en sistemas de retroalimentación para En primer lugar, se seleccionó un material ligero pero rígido
control de motores [10]. para asegurar que la rueda mantuviera una geometría
balanceada durante el giro. Para su manufactura se
• Driver de motor TB6612FNG: El TB6612FNG es un consideraron únicamente procesos accesibles como corte láser
controlador dual de motores DC que permite manejar e impresión 3D, por lo que se eligió madera MDF de 5 mm
corrientes de hasta 1.2 A continuos por canal. como material base debido a su facilidad de corte, rigidez,
Proporciona control de dirección y velocidad mediante ligereza y adaptabilidad a geometrías bidimensionales.
señales PWM y es ampliamente utilizado en
Gracias a la precisión de la máquina láser, fue posible reducir
aplicaciones embebidas gracias a su eficiencia y
protección integrada contra sobrecorriente y significativamente las variaciones irregulares en la
sobrecalentamiento [11]. distribución de masa que podrían afectar la estabilidad de la
rueda; estas irregularidades son más probables en prototipos
• Método de Ziegler–Nichols: Es una técnica empírica impresos en 3D, además de que implican un costo de
ampliamente utilizada para sintonizar controladores manufactura mayor.
PID. Consiste en llevar al sistema al borde de la
Tras un análisis dimensional del motor, se diseñó la
estabilidad aumentando la ganancia proporcional hasta
obtener oscilaciones sostenidas. A dicho valor se le estructura mostrada en la Figura 4. El primer prototipo tomó
denomina ganancia última 𝐾𝑢 , y el período de como referencia diseños de ruedas de reacción
oscilación correspondiente se conoce como período experimentales, en los cuales se integran masas distribuidas
último 𝑇𝑢 . A partir de estos dos parámetros se calculan alrededor de la circunferencia para incrementar el momento de
las ganancias del controlador PID siguiendo reglas inercia. No obstante, se estimó que añadir peso en la periferia
establecidas por Ziegler y Nichols, las cuales podría comprometer la capacidad del motor debido a sus
proporcionan una respuesta rápida y razonablemente limitaciones de torque. Por ello, se optó por escalar la
estable sin requerir un modelo matemático exacto del geometría general de la rueda para incrementar su masa total
sistema. Este método es especialmente útil en sistemas sin sobrecargar el extremo exterior de la misma.
donde la dinámica es desconocida o difícil de modelar
[12].

III. METODOLOGÍA
En esta sección se presenta la metodología empleada para
la implementación de un controlador PID para la planta del
circuito de una rueda de reacción.

1. Descripción general del procedimiento

La metodología empleada para el desarrollo del sistema de

control basado en una rueda de reacción se centró en el
modelado experimental, la implementación del hardware y el
Figura 4. Prototipo inicial de rueda de reacción - modelado en
ajuste progresivo de las ganancias del controlador. Debido a SolidWorks.
que la dinámica del prototipo presenta características no
ideales como: fricción, discretización del sensor y variaciones Durante las pruebas iniciales de funcionamiento, se
en la respuesta del motor. Gran parte del diseño del identificó la posibilidad de desgaste del material en la zona de
controlador se realizó mediante la estimación del acoplamiento directo entre el eje del motor y la rueda. Esto
comportamiento real del sistema y la validación directa a motivó un rediseño que incluyó la incorporación de un cople
través de pruebas experimentales. impreso en 3D (Figura 5), el cual permitió conectar el motor
a la rueda desde puntos más rígidos y estructuralmente
2. Diseño de la rueda de reacción estables. Asimismo, se ajustaron dimensiones específicas
La rueda de reacción constituye el elemento central del como el diámetro de la rueda para optimizar su desempeño
sistema, ya que es la encargada de generar el momento angular mecánico (Figura 6).
necesario para modificar la orientación del prototipo. Su
diseño se basó en criterios mecánicos y dinámicos que
garantizan un funcionamiento estable y con la inercia
suficiente para producir el torque requerido.

3
 Control

3. Materiales utilizados

• Raspberry Pi Pico
• Sensor MPU-6050 (acelerómetro + giróscopo)
• Driver de motor TB6612FNG
• Motor de 6 V con encoder
• Protoboard
• Jumpers de conexión (macho-macho y macho-
hembra)
• Cables para protoboard
• Capacitor de 100 µF
• Fuente de voltaje externa
Figura 5. Diseño de cople implementado.
4. Implementación del sistema de control

La implementación del sistema de control consistió en

integrar los elementos electrónicos, mecánicos y de
programación necesarios para accionar la rueda de reacción y
regular la orientación del prototipo. La Raspberry Pi Pico se
utilizó como unidad de procesamiento principal debido a su
baja latencia, capacidad de generación de señales PWM y
facilidad de programación en MicroPython o C. Esta tarjeta
fue responsable de ejecutar el algoritmo PID y procesar la
información de retroalimentación en tiempo real.

El sistema de medición se basó en un sensor inercial MPU-

6050 (Figura 8), el cual integra un acelerómetro y un
Figura 6. Diseño modificado de rueda.
giróscopo triaxiales. A través de comunicación I2C, el sensor
Esta etapa culminó con el diseño final presentado a envió datos de aceleración y velocidad angular a la Raspberry
continuación. Además, se verificó que el centro de masa Pi Pico.
estuviera adecuadamente distribuido para minimizar
desbalances que pudieran generar vibraciones u oscilaciones
no deseadas. Como resultado, la geometría final permitió que
el sistema alcanzara velocidades angulares adecuadas y
generara el torque suficiente para estabilizar la plataforma ante
perturbaciones. El resultado de la estructura final se presenta
en la Figura 7.

Figura 8. Sensor MPU-6050 – Posicionado en base del brazo de la rueda.

El actuador principal fue un motor DC de 6 V con encoder

integrado, utilizado para girar la rueda de reacción. El encoder
permitió medir la velocidad angular del rotor, lo que facilitó la
verificación del comportamiento dinámico del motor y
permitió descartar condiciones de saturación o pérdida de
control. El motor fue alimentado y controlado mediante un
driver TB6612FNG, el cual ofrece control bidireccional y
Figura 7. Diseño final de rueda de reacción. modulación PWM para ajustar la velocidad del motor de

4
 Control

manera precisa y eficiente, además de proteger el circuito ante del sistema alrededor de un punto de operación. Esta
sobrecorrientes. simplificación permitió expresar la dinámica en forma de
función de transferencia y emplear técnicas clásicas de análisis
Toda la electrónica fue montada en un protoboard, de estabilidad.
empleando cables tipo jumper macho–macho y macho–
hembra para realizar las conexiones entre la Raspberry Pi Pico, A partir del modelo analítico del motor y la interacción de
el sensor, el driver y la fuente de alimentación. Se integró un momento angular entre la rueda y la plataforma, se obtuvo una
capacitor de 100 µF cercano al driver con el propósito de función de transferencia equivalente, la cual fue implementada
mitigar caídas de voltaje inducidas por los cambios bruscos de y evaluada en MATLAB. La Figura 10 (correspondiente a la
corriente del motor. Finalmente, una fuente de alimentación función de transferencia mostrada en MATLAB) presenta el
externa proporcionó la energía necesaria para el motor y los modelo del sistema. Posteriormente, se generaron las
componentes electrónicos, asegurando una operación estable respuestas al impulso y al escalón, mostradas en la Figura 11,
y aislada del consumo de la Raspberry Pi Pico. Todo el donde se observa un crecimiento exponencial de la señal; este
conjunto montado se muestra en la Figura 9. comportamiento confirma la naturaleza inestable del sistema
sin control, característica consistente con mecanismos de
orientación basados en reacción. De igual forma, el diagrama
de Bode obtenido (Figura 11) evidencia la presencia de polos
en el semiplano derecho, ratificando la necesidad de un
controlador para garantizar la estabilidad del sistema real.

Motor

MPU-6050 Figura 10. Función de transferencia evaluada en MATLAB.

TB6612FNG
Raspberry Pi Pico

Capacitor de 100 µF

Figura 9. Conexión de hardware.

La integración de todos estos elementos permitió conformar Figura 11. Respuestas del sistema ante escalón unitario e impulso.
un sistema de control funcional, capaz de interpretar la
orientación del prototipo y modificarla mediante el Este análisis previo fue indispensable para comprender la
accionamiento de la rueda de reacción en tiempo real. estructura dinámica del prototipo y para justificar el uso de un
controlador PID. Aunque el modelo linealizado no captura
Además de la implementación física del sistema, fue todas las no linealidades presentes en el sistema físico, sí
necesario comprender la dinámica del prototipo para facilitar proporciona una representación adecuada para predecir su
el diseño del controlador. La rueda de reacción, en conjunto comportamiento general en lazo abierto y orientar el proceso
con el motor, la estructura y el sensor, constituye un sistema de sintonización. El uso de MATLAB permitió visualizar de
inherentemente no lineal debido a la fricción variable, el forma clara estas tendencias, validar el modelo analítico y
acoplamiento dinámico entre la rueda y la plataforma, la establecer las bases que guiaron el ajuste de las ganancias
saturación del actuador y la cuantización del MPU-6050. Sin descrito en la siguiente sección.
embargo, para efectos de análisis y diseño de controladores
lineales, se realizó una aproximación mediante la linealización 5. Ajuste de las ganancias del controlador

5
 Control

El proceso de sintonización se llevó a cabo en tres etapas: Tras completar las etapas de estimación inicial, Ziegler–
Nichols y ajuste fino, se obtuvieron los valores óptimos de las
5.1 Estimación inicial a partir del comportamiento del ganancias del controlador PID. Los valores finales
sistema seleccionados fueron:
Antes de aplicar un método formal de sintonización, se
• 𝐾𝑝 = 0.3
realizó una observación directa del comportamiento del
• 𝐾𝑖 = 1.5
prototipo ante pequeñas perturbaciones manuales. Este
análisis permitió identificar características cualitativas del • 𝐾𝑑 = 0.0001
sistema, como la inercia dominante generada por la rueda de Esta configuración permitió alcanzar un balance adecuado
reacción, el retardo entre la señal de control y la respuesta entre rapidez de respuesta, estabilidad dinámica y capacidad
física del motor, y el nivel de amortiguamiento natural. Con de corrección ante perturbaciones externas. Con estas
base en estas observaciones se propusieron valores iniciales ganancias el sistema pudo regular el ángulo de la plataforma
para las ganancias proporcional, integral y derivativa, los sin presentar oscilaciones sostenidas, saturación del actuador
cuales sirvieron como punto de partida para la sintonización o inestabilidad, logrando un desempeño satisfactorio bajo las
formal. condiciones de prueba establecidas.
5.2 Método de Ziegler–Nichols IV. RESULTADOS
Posteriormente se aplicó el método de Ziegler–Nichols en La implementación del sistema de control basado en una
lazo cerrado, una técnica empírica utilizada comúnmente rueda de reacción permitió evaluar tanto el comportamiento
cuando el modelo matemático de la planta no está dinámico del prototipo como la efectividad del controlador
completamente definido o cuando se requiere una estimación PID diseñado. En primer lugar, las pruebas iniciales del
rápida y funcional de las ganancias del controlador. Para ello, modelo linealizado mostraron que la planta presentaba
el valor de la ganancia proporcional 𝐾𝑝 se incrementó inestabilidad en lazo abierto, característica observada en la
respuesta al impulso y al escalón obtenida mediante
progresivamente mientras se desactivaban los términos
MATLAB (Figura 10). Ambas respuestas evidenciaron un
integral y derivativo, hasta que el sistema mostró oscilaciones
crecimiento exponencial de la señal, lo cual coincide con la
sostenidas. El valor de 𝐾𝑝 que generó dichas oscilaciones se
presencia de polos en el semiplano derecho identificados en el
identificó como la ganancia última 𝐾𝑢 , y el período de
análisis de Bode. Este comportamiento confirma que el
oscilación resultante como el período último 𝑇𝑢 . sistema físico, al depender del principio de conservación del
A partir de estos parámetros se calcularon las ganancias momento angular, carece de mecanismos pasivos de
preliminares del controlador PID conforme a las reglas amortiguamiento que limiten la divergencia ante
propuestas por Ziegler y Nichols. Estas ganancias perturbaciones. Esto justifica plenamente la necesidad de un
proporcionaron una primera configuración estable y sirvieron controlador robusto para mantener la plataforma en equilibrio.
como referencia para la etapa de ajuste fino. Durante la etapa experimental, se observó que la Raspberry
5.3 Ajuste por prueba y error Pi Pico generó señales PWM estables y consistentes (Figura
12), como se aprecia en la gráfica obtenida mediante
Si bien el método de Ziegler–Nichols ofrece valores osciloscopio. La señal presenta una forma rectangular bien
iniciales razonables, la dinámica real del sistema presentó definida a una frecuencia aproximada de 8.34 kHz, lo que
características no lineales y fenómenos no modelados, como indica un adecuado funcionamiento del driver TB6612FNG y
fricción variable, ruido del sensor, pequeñas desalineaciones del proceso de modulación. Esta estabilidad en la señal de
mecánicas y fluctuaciones en la velocidad del motor. Por ello control resultó esencial para garantizar que el motor
fue necesario complementar la sintonización con un proceso respondiera de forma lineal al esfuerzo solicitado por el
iterativo de prueba y error. controlador, minimizando variaciones no deseadas en la
velocidad de la rueda de reacción.
Durante esta etapa se variaron manualmente las ganancias
𝐾𝑝 , 𝐾𝑖 y 𝐾𝑑 observando en tiempo real la respuesta del sistema
ante perturbaciones. El objetivo fue reducir el sobreimpulso,
mejorar la estabilidad y minimizar las vibraciones. Este
proceso permitió adaptar el control PID directamente al
comportamiento físico del prototipo, logrando finalmente un
desempeño más robusto y suave.

5.4 Valores finales de ganancia

6
 Control

Figura 12. respuesta PWM.

Una vez implementado el controlador PID, se observó una

mejora significativa en la estabilidad del sistema. Con las
ganancias finales seleccionadas (𝐾p=0.3, 𝐾i=1.5,
𝐾d=0.0001), el prototipo fue capaz de corregir perturbaciones
moderadas y mantener el ángulo dentro de un rango operativo
seguro. El término integral desempeñó un papel importante al
compensar errores constantes generados por la fricción del
Figura 13. Resultado final Rueda de Reacción controlada con controlador
motor y pequeñas desviaciones del sensor, mientras que el PID.
término derivativo, aunque pequeño, contribuyó a suavizar la
respuesta dinámica reduciendo vibraciones. Además, el VI. CONCLUSIONES
comportamiento final evidenció una reducción notable del Los resultados obtenidos permiten concluir que el sistema
sobreimpulso y una respuesta más rápida respecto a las de control desarrollado fue capaz de estabilizar una plataforma
primeras fases de prueba. dinámica utilizando únicamente una rueda de reacción como
actuador principal.
Finalmente, la comparación entre el comportamiento
simulado y el experimental mostró una correlación adecuada: La implementación del controlador PID sobre la Raspberry
aunque el modelo linealizado no captura todas las no Pi Pico, utilizando retroalimentación proporcionada por el
linealidades del sistema real, sí permitió predecir tendencias MPU-6050 y el encoder del motor, dio lugar a un desempeño
generales como la inestabilidad en lazo abierto y la necesidad estable y consistente. El proceso de sintonización demostró
de una acción integral dominante. Esto valida el uso de que una estrategia combinada —modelo linealizado + método
modelos aproximados para sistemas complejos, siempre de Ziegler-Nichols + ajuste fino experimental— es adecuada
complementados con sintonización experimental y para sistemas no lineales. Las señales PWM observadas
verificación práctica. confirmaron un control confiable del actuador, mientras que el
comportamiento final del prototipo evidenció correcciones
En conjunto, los resultados obtenidos demuestran que el rápidas y sin oscilaciones significativas.
diseño del controlador fue adecuado para estabilizar la
plataforma mediante una rueda de reacción, confirmando la Asimismo, el proyecto permitió validar que, aunque el
viabilidad del enfoque y resaltando la importancia de integrar modelo linealizado no captura todas las complejidades del
modelado, simulación y pruebas físicas en el desarrollo de sistema real, sí proporciona una base suficiente para orientar
sistemas de control. el diseño del controlador. La correlación entre simulaciones y
resultados experimentales confirma la utilidad del modelado
preliminar como herramienta de apoyo en la implementación
práctica.

En conjunto, se demostró que una rueda de reacción es un

mecanismo eficaz para regular la orientación de una
plataforma, y que la correcta integración entre hardware,
software y teoría de control es fundamental para sistemas
dinámicos inestables.

7
 Control

VII. REFERENCIAS línea]. Available: [Link]

terms/introduction-chemical-
engineering/proportional-integral-controller.
[1] First Robotics Competition, «Control System
Basics,» [En línea]. Available:
[Link]
anced-controls/introduction/control-system-
[Link]. VIII. ANEXOS
1. CÓDIGO
[2] Deingenierias, «Qué es una función de
transferencia en ingeniera de control,» 30 Abril #################################################
# Reaction Wheel Inverted Pendulum
2025. [En línea]. Available: [Link]/xTLjn.
# Created by: Abigail y Angel
# Hardware:
[3] B. C. Kuo, Sistemas de Control Automático, # MPU6050 IMU
Pearson Educación. # TB6612FNG H-Bridge
# Raspberry Pi Pico
[4] K. Ogata, Ingeniería de control moderna, # Motor de 6 V con encoder
#
PEARSON EDUCACIÓN, 2010. #################################################

[5] J. L. MERIAM, L. G. KRAIGE y J. N. BOLTON,

ENGINEERING MECHANICS DYNAMICS, #settings
ANGLE_FILTER_G = 0.999 #gyro portion of complementary filter
WILEY, 2015. [0...1]
ANGLE_LIMIT = 18 #stop program / start rise up sequence [deg]
[6] D. Halliday, R. Resnick y J. Walker, RISEUP_END_ANGLE = 12 #stop rise up sequence and start balancing
FUNDAMENTALS OF PHYSICS, WILEY, 2007. [deg]
ANGLE_FIXRATE = 5.5 #variate target angle [deg/s]
ANGLE_FIXRATE_2 = 0.001 #reduce continuous rotation
[7] B. Wie, Space Vehicle Dynamics and Control,
KP = 0.3 #PID proportional factor
Arizona: American Institute of Aeronautics and KI = 1.5 #PID integral factor
Astronautic, 1998. KD = 0.0001 #PID derivative factor
MOTOR_R = 13 #motor resistance [Ohm]
[8] IvenSense, «MPU-6000 and MPU-6050 Product MOTOR_Ke = 0.26 #motor back EMF constant [Vs/rad]
SUPPLY_VOLTAGE = 6 #battery box voltage [V]
Specification Revision 3.4,» 19 Agosto 2013. [En WHEEL_AV_INTERVAL = 100 #wheel angular velocity calculation
línea]. Available: [Link] interval [ms]
content/uploads/2015/02/MPU-6000- SLEEP_TIME = 1 #main loop sleep [ms]
[Link]. risingUp = True
IN_TACHOA = 18
IN_TACHOB = 19
[9] A. Hughes, Electric Motors and Drive OUT_DIR1 =5
Fundamentals, Types and Applications, OUT_DIR2 =6
ELSEVIER, 2006. OUT_PWM =7
DUTY = 125000
CNT = 155
[10] W. Bolton, INSTRUMENTATION AND ###########################################################
CONTROL SYSTEMS, ELSEVIER, 2021. #
from imu import MPU6050
[11] TOSHIBA, TB6612FNG DATASHEET, import time
TOSHIBA, 2014. import mia
########
from time import sleep#, perf_counter_ns
[12] R. C. Dorf y R. H. Bishop, Modern Control #from datetime import datetime
Systems, PEARSON EDUCATION, 2017. import math
#import signal
[13] B. G. Liptak, Instrument Engineers' Handbook, from machine import Pin, I2C,PWM
#from mpu9250_jmdev.registers import *
Process Control and Optimization, ISA, 2005. #from mpu9250_jmdev.mpu_9250 import MPU9250
#from gpiozero import DigitalInputDevice, DigitalOutputDevice,
[14] Fiveable, « Proportional-Integral Controller – PWMOutputDevice
Intro to Chemical Engineering,» 31 Julio 2024. [En #from [Link] import PiGPIOFactory

8
 Control

### tachoCount += 1
def perf_counter_ns(): def tachoB_falling(pin):
return time.time_ns()
### global tachoBValue
tachoBValue = 0
#prepare IMU sensor
#imu = MPU9250(bus=1, gfs=GFS_250, afs=AFS_2G) def irq_A(pin):
#[Link] = [0.013221153846153846, -0.030993339342948716, if [Link]() == 1:
0.004657451923076872] tachoA_rising(pin)
#[Link] = [1.3726094563802083, 1.2428309122721355, else:
0.00033772786458334] tachoA_falling(pin)
#[Link]()
i2c = I2C(0,sda=Pin(0),scl=Pin(1),freq=400000) def irq_B(pin):
imu=MPU6050(i2c) if [Link]() == 1:
#sleep(5) tachoB_rising(pin)
#mia.cal_a(5) else:
a=[ 0.009991597 , 0.013971089 , 0.05842058 ] tachoB_falling(pin)
g=[ -1.74752624 , 0.4420805 , 0.83425224 ] """
mia.offset_MPU(a,g) tachoA.when_activated = tachoA_rising
tachoA.when_deactivated = tachoA_falling
#prepare motor controller tachoB.when_activated = tachoB_rising
#my_factory = PiGPIOFactory() #hardware pwm supported tachoB.when_deactivated = tachoB_falling
#motorPWM = PWMOutputDevice(12, pin_factory=my_factory)
#motorDIR1 = DigitalOutputDevice(5) tachoA.IRQ_HIGH_LEVEL = tachoA_rising
#motorDIR2 = DigitalOutputDevice(6) tachoA.IRQ_LOW_LEVEL = tachoA_falling
#[Link] = 8000 tachoB.IRQ_HIGH_LEVEL = tachoB_rising
#[Link] = 0 tachoB.IRQ_LOW_LEVEL = tachoB_falling
motorPWM=PWM(OUT_PWM) """
[Link](8000) [Link](trigger=Pin.IRQ_RISING | Pin.IRQ_FALLING,
motorPWM.duty_ns(0) handler=irq_A)
motorDIR1=Pin(OUT_DIR1,[Link]) #[Link](trigger=Pin.IRQ_FALLING, handler=tachoA_falling)
motorDIR2=Pin(OUT_DIR2,[Link]) [Link](trigger=Pin.IRQ_RISING | Pin.IRQ_FALLING,
[Link](0) handler=irq_B)
[Link](0) #[Link](trigger=Pin.IRQ_FALLING, handler=tachoB_falling)
"""
#prepare tachometers #exit program when Ctrl-C is pressed
#tachoA = DigitalInputDevice(22) exitRequested = False
#tachoB = DigitalInputDevice(23) def sigintHandler(sig, frame):
tachoA = Pin(IN_TACHOA,[Link],Pin.PULL_UP) print("Ctrl-C pressed, exit program")
tachoB = Pin(IN_TACHOB,[Link],Pin.PULL_UP) global exitRequested
tachoCount = 0 exitRequested = True
tachoAValue = [Link]
tachoBValue = [Link] [Link]([Link], sigintHandler)
def tachoA_rising(pin): [Link]([Link], sigintHandler)
global tachoCount """
global tachoAValue
global tachoBValue print("program started")
tachoAValue = 1 sleep(0.1)
if tachoBValue == 0:
#A in rising edge and B in low value startTime = perf_counter_ns()
# => direction is clockwise (shaft end perspective) prevLoopTime = perf_counter_ns()
tachoCount += 1 prevTachoTime = perf_counter_ns()
else: prevPrintTime = perf_counter_ns()
tachoCount -= 1 loopCount = 0
def tachoA_falling(pin): gyroAngle = float('nan')
global tachoAValue measuredAngle = float('nan')
tachoAValue = 0
def tachoB_rising(pin): prevTachoCount = 0
wheelAV = 0
global tachoCount targetAngle = 0
global tachoAValue error = 0
global tachoBValue prevError = 0
tachoBValue = 1 integral = 0
if tachoAValue == 0: derivative = 0
tachoCount -= 1 PIDoutput = 0
else: motorCtrl = 0

9
 Control

#logData =
[["secondsSinceStart","accAngle","gyroAngle","measuredAngle","target pulses = tachoCount - prevTachoCount
Angle", prevTachoCount = tachoCount
# "tachoCount","wheelAV","error","integral","derivative", cycles = pulses / CNT #360 pulses per rotation
# "PIDoutput","motorCtrl"]] wheelAV = cycles / tachoTimeDelta * 2 * [Link] #[rad/s]

exitRequested=False if not risingUp:

while not exitRequested:
#variate target angle
timeDelta = (perf_counter_ns() - prevLoopTime) / 1e9 #[sec] if measuredAngle < targetAngle:
prevLoopTime = perf_counter_ns() targetAngle += ANGLE_FIXRATE * timeDelta
secondsSinceStart = (perf_counter_ns() - startTime) / 1e9 else:
targetAngle -= ANGLE_FIXRATE * timeDelta
#read accelerometer
ax, ay, az = [Link]() #[G] #reduce continuous rotation
accAngle = [Link](-ax / [Link](pow(ay, 2) + pow(az, 2))) * 180 / targetAngle -= ANGLE_FIXRATE_2 * wheelAV * timeDelta
[Link] #[deg]
#PID controller
#read gyroscope error = targetAngle - measuredAngle
gx, gy, gz = [Link]() #[deg/s] integral += error * timeDelta
gyroAngleDelta = gz * timeDelta derivative = (error - prevError) / timeDelta
if [Link](gyroAngle): gyroAngle = accAngle prevError = error
gyroAngle += gyroAngleDelta #[deg] PIDoutput = KP * error + KI * integral + KD * derivative

#calculate arm angle (complementary filter) #compensate for motor back EMF voltage
if [Link](measuredAngle): measuredAngle = accAngle current = -PIDoutput
measuredAngle = (ANGLE_FILTER_G * (measuredAngle + voltage = MOTOR_R * current + MOTOR_Ke * wheelAV
gyroAngleDelta) +
(1-ANGLE_FILTER_G) * accAngle) #[deg] #convert voltage to pwm duty cycle
#safety check motorCtrl = voltage / SUPPLY_VOLTAGE
if abs(measuredAngle) >= ANGLE_LIMIT: motorCtrl=-PIDoutput
if secondsSinceStart < 0.001: #drive motor
print("START RISE UP SEQUENCE") motorCtrl = min(max(motorCtrl, -1), 1) #limit range to -1...1
risingUp = True #[Link] = abs(motorCtrl)
elif not risingUp: motorPWM.duty_ns(int(DUTY*abs(motorCtrl)))
print("PROGRAM STOPPED, angle is too large: %.2f" % [Link]((motorCtrl > 0))
(measuredAngle)) [Link]((motorCtrl < 0))
break
"""
#rise up sequence #log data for post analysis
if risingUp: [Link]([secondsSinceStart, accAngle, gyroAngle,
if secondsSinceStart < 1.0: measuredAngle, targetAngle,
#speed up reaction wheel to full speed tachoCount, wheelAV, error, integral, derivative,
#[Link] = 1.0 PIDoutput, motorCtrl])
motorPWM.duty_ns(int(DUTY*1.0))
[Link]((measuredAngle < 0)) #debug print
[Link]((measuredAngle > 0)) if (perf_counter_ns() - prevPrintTime) / 1e9 >= 1.0:
elif secondsSinceStart < 1.5: secondsSinceLastPrint = (perf_counter_ns() - prevPrintTime) / 1e9
#change direction using full power prevPrintTime = perf_counter_ns()
#[Link] = 1.0 loopInterval = secondsSinceLastPrint / loopCount * 1000
motorPWM.duty_ns(int(DUTY*1.0)) loopCount = 0
[Link]((measuredAngle > 0)) print("measuredAngle: %.2f, motorCtrl: %.2f, loopInterval: %.2f ms"
[Link]((measuredAngle < 0)) % (measuredAngle, motorCtrl, loopInterval))
#wait until close to top position, then start balancing """
if abs(measuredAngle) < RISEUP_END_ANGLE: sleep(SLEEP_TIME / 1000)
print("END RISE UP SEQUENCE") loopCount += 1
risingUp = False
else: #stop motor
print("RISE UP TIMEOUT") #[Link] = 0
break motorPWM.duty_ns(int(DUTY*0.0))
[Link](0)
#calculate wheel angular velocity [Link](0)
if (perf_counter_ns() - prevTachoTime) / 1e6 >=
WHEEL_AV_INTERVAL: """
tachoTimeDelta = (perf_counter_ns() - prevTachoTime) / 1e9 #[sec] #write log data to file
prevTachoTime = perf_counter_ns() print("log size", len(logData), "rows")

10
 Control

if len(logData) > 0:
filename = "[Link]"
#filename = "datalog_" +
[Link]().strftime("%Y%m%d_%H%M%S") + ".dat"
print("write to file:", filename)
file = open(filename, "w")
for logLine in logData:
for value in logLine:
[Link](str(value) + ' ')
[Link]('\n')
[Link]()
"""

print("program ended")

2. FUNCIONAMIENTO

[Link]
UATR1U1TfWWMq1u/view?usp=drivesdk

11