Guimbal

A simple guimbal with ESP32 and MPU6050

In the world of DIY electronics, gimbals are a fascinating way to stabilize cameras or sensors, enabling smooth movement and precise control. This article explores how to create a simple yet effective gimbal using an ESP32 microcontroller, an MPU6050 accelerometer/gyroscope module, and a pair of servos, all programmed with MicroPython.

We’ll walk through each step of the process—from setting up the hardware to writing and optimizing the code. Along the way, you'll learn how to harness the MPU6050's motion-sensing capabilities and control servo motors to achieve real-time stabilization. Whether you’re looking to build a camera stabilizer or a motion-tracking device, this project offers a great introduction to using MicroPython with sensors and actuators.

Material used

ESP332 microcontroller with microPython firmware;
GY-521 accelerometer/gyroscope module with MPU6050 sensor;
Couple of 9g RC servos;
Some jumpers, wiring, and stuff;
A protoboard;
Simulator.

ESP32

The ESP32 is a powerful, low-cost microcontroller with built-in Wi-Fi and Bluetooth capabilities, making it an ideal choice for Internet of Things (IoT) applications, embedded systems, and robotics. Its dual-core processor, extensive GPIO options, and support for a variety of communication protocols enable versatile hardware integration.

ESP32 wroom

ESP32

ESP32 pinout

pinout

MicroPython

The ESP32 microcontroller, known for its integrated Wi-Fi and Bluetooth capabilities, has gained popularity in embedded systems and IoT applications due to its versatility and low cost. Running MicroPython firmware on the ESP32 simplifies development by providing a high-level, Python-based interface for hardware control. MicroPython is a lightweight implementation of Python 3, optimized for resource-constrained devices, enabling rapid prototyping and deployment of complex applications with minimal code. It supports access to ESP32’s peripherals, such as GPIOs, I²C, SPI, and PWM, through intuitive commands, which reduces the need for traditional low-level programming in C or C++. Furthermore, MicroPython’s extensive libraries and community support make it a practical choice for educational projects and rapid IoT development, bridging the gap between software and hardware design.

Overview

This combination of ESP32’s hardware capabilities and MicroPython’s flexibility empowers developers and researchers to create robust, connected solutions with minimal overhead. The following sections will explore their integration, highlighting how MicroPython simplifies hardware interaction and enhances the development process for embedded systems.Gy

GY-521

The GY-521 is a widely used breakout board based on the MPU6050, a 6-axis motion tracking sensor that integrates a 3-axis gyroscope and a 3-axis accelerometer. It is designed for applications requiring precise motion detection and orientation control, such as robotics, wearable devices, and stabilization systems. The MPU6050's on-chip Digital Motion Processor (DMP) allows for sensor fusion, providing orientation data without the need for complex external calculations.

MPU6050 based brickout GY-521

The GY-521 operates over an $I^{2}C$ interface, making it compatible with a variety of microcontrollers, including the ESP32. It offers selectable sensitivity ranges for both the accelerometer and gyroscope, allowing users to optimize performance based on specific application needs. Additionally, the module includes features like a voltage regulator and pull-up resistors, enabling operation in systems with different voltage levels.

This sensor's combination of affordability, compact design, and rich functionality has made it a standard choice in academic research and practical projects that require real-time motion analysis. In the context of embedded systems, the GY-521 is particularly valuable for its ability to provide stable and accurate data, facilitating advanced control and feedback mechanisms.

Guimbal

A gimbal is a mechanical device designed to stabilize and control the orientation of an object, such as a camera or sensor, by allowing rotation along one or more axes. By actively compensating for motion, gimbals maintain a stable platform, even when subjected to external disturbances. This stabilization is crucial in various fields, including aerospace, robotics, videography, and military applications.

3-axis guimbal with a gopro hero4 camera

In cinematography, gimbals ensure smooth camera movements by counteracting hand tremors and vibrations, enabling high-quality, professional footage. This technology is also vital in drone operations, where gimbals stabilize onboard cameras during flight, enhancing image clarity and data accuracy. In aerospace and military contexts, gimbals support precision targeting and surveillance by maintaining stable sensor alignment despite platform motion or environmental conditions¹².

The stabilization process typically employs feedback control systems, such as Proportional-Integral-Derivative (PID) controllers, and integrates advanced sensors, including gyroscopes and accelerometers, to detect and correct movement in real-time³⁴. Recent advancements have introduced brushless motors and sophisticated control algorithms, enhancing gimbal performance in complex environments like space and underwater operations⁵.

The versatility and effectiveness of gimbals have made them indispensable tools in applications requiring high precision and stability, contributing significantly to advancements in imaging, data collection, and motion control.

Wiring

wiring

Calculate pitch, row, and, yaw with accelerometer data

Pitch

$P = \arctan(\frac{accelX}{\sqrt{accelY^{2} + accelZ^{2}}})$

P $\to$ accelerometer calculated pith angle;
R $\to$ accelerometer calculated roll angle;
Y $\to$ accelerometer calculated yaw angle;
accelX $\to$ x axis from accelerometer;
accelY $\to$ y axis from accelerometer;
accelZ $\to$ z axis from accelerometer.

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import math
pitch = math.degrees(math.atan(accel_x / math.sqrt(accel_y**2 + accel_z**2)))

Roll

$R = \arctan(\frac{accelY}{\sqrt{accelX^{2} + accelZ^{2}}})$

Alternatively:

$R = \arctan(\frac{accelY}{accelZ})$

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import math
roll  = math.degrees(math.atan(accel_y / math.sqrt(accel_y**2 + accel_z**2)))

Yaw

$Y = \arctan(\frac{\sqrt{accelX^{2} + accelY^{2}}}{accelZ})$

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import math
 yaw   = math.degrees(math.atan(math.sqrt(accel_x**2 + accel_y**2)/ accel_y))

Calculate pitch, row, and, yaw with gyroscope data

Pitch

$GP = gyroY \cdot \delta(t)$

GP $\to$ gyro calculated pitch angle;
GR $\to$ gyro calculated roll angle;
GY $\to$ gyro calculated yaw angle;
$\delta(t) \to$ time difference between;
gyroX $\to$ x axis from the gyroscope;
gyroY $\to$ y axis from the gyroscope;
gyroZ $\to$ z axis from the gyroscope.

GP = gyroY * (t1 - t0)

Roll

$GR = gyroX \cdot \delta(t)$

gr = gyroX * (t1 - t0)

Yaw

$GY = gyroZ \cdot \delta(t)$

gr = gyroZ * (t1 - t0)

Calculate yaw with a magnetometer with 9 DORF MPU

$MX = magX \cdot \cos(P) + magZ \cdot \sin(P)$

MX $\to$ calculated angle from the magnetometer;
MY $\to$ calculated angle from the magnetometer;
magX $\to$ angle readings from the magnetometer.

import math

MX = magX * math.cos(P) + magZ * math.sin(P)

$MY = magX \cdot \sin(R) \cdot \sin(P) + \cos(R) - magZ \cdot sin(R) \cdot \sin(R)$

import math

MY = magX * math.sin(R) * math.sin(P) + math.cons(R) - magZ * math.sin(R) * math.sin(R)

$MYaw = \arctan(\frac{MY}{MX})$

Creating a simple 2-axis guimbal

Link to wokwi project on simple guimbal.

# Frederico Sales
# <frederico@fredericosales.eng.br>
# 2022
#

# imports
import machine
import time
from mpu6050 import MPU6050
import math
from machine import Pin, PWM

# Set up the I2C interface
i2c = machine.I2C(1, sda=Pin(21), scl=Pin(22))

# Initialize servo motors
SERVO_PIN_PITCH = 14                                # Servo controlling the pitch (GPIO14)
SERVO_PIN_ROLL = 15                                 # Servo controlling the roll  (GPIO15)
pitch_servo = PWM(Pin(SERVO_PIN_PITCH), freq=50)    # Pitch servo PWM signal
roll_servo = PWM(Pin(SERVO_PIN_ROLL), freq=50)      # Row servo PWM signal
SERVO_MIN = 40                                      # Minimum pulse width
SERVO_MAX = 115                                     # Maximum pulse width

# Function to map sensor data to servo angle
def map_value(x, in_min, in_max, out_min, out_max):
    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

# Function to set servo angle (0 to 180 degrees)
def set_servo_angle(servo, angle):
    pulse_width = map_value(angle, 0, 180, SERVO_MIN, SERVO_MAX)
    servo.duty(pulse_width)

# Set up the MPU6050 class 
mpu = MPU6050(i2c)

# wake up the MPU6050 from sleep
mpu.wake()

while True:
    # Read the accelerometer data (pitch and roll)
    accel_data = mpu.read_accel_data()

    #print(accelerometer: accel_data)
    accel_x = accel_data[0]
    accel_y = accel_data[1]
    accel_z = accel_data[2]
    #print(f"axis x: {accel_x} \t axis y: {accel_y} \t axis z: {accel_z}")

    # Calculate pitch and yaw (basic sensor fusion can be added here) (pitch - row - yaw)
    pitch = math.degrees(math.atan(accel_x / math.sqrt(accel_y**2 + accel_z**2)))
    roll  = math.degrees(math.atan(accel_y / math.sqrt(accel_y**2 + accel_z**2)))
    yaw   = math.degrees(math.atan(math.sqrt(accel_x**2 + accel_y**2)/ accel_y))

    # Map the pitch and yaw to servo angles
    pitch_angle = map_value(pitch, -90, 90, 0, 180)
    roll_angle = map_value(roll, -90, 90, 0, 180)
    yaw_angle = map_value(yaw, -180, 180, 0, 360)
    print(f"pitch angle: {pitch_angle} \t roll angle: {roll_angle} \t yaw angle: {yaw_angle} \t Temp.: {mpu.read_temperature()}")

    # Set the servo angles to control the gimbal
    set_servo_angle(pitch_servo, pitch_angle)
    set_servo_angle(roll_servo, roll_angle)

    # Wait a short time to allow the motors to move
    time.sleep(0.05)

mpu6050 python class

"""
A lightweight MicroPython implementation for interfacing with an MPU-6050 via I2C. 
Author: Tim Hanewich - https://github.com/TimHanewich
Version: 1.0
Get updates to this code file here: https://github.com/TimHanewich/MicroPython-Collection/blob/master/MPU6050/MPU6050.py

License: MIT License
Copyright 2023 Tim Hanewich
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
"""

import machine

class MPU6050:
    """Class for reading gyro rates and acceleration data from an MPU-6050 module via I2C."""

    def __init__(self, i2c:machine.I2C, address:int = 0x68):
        """
        Creates a new MPU6050 class for reading gyro rates and acceleration data.
        :param i2c: A setup I2C module of the machine module.
        :param address: The I2C address of the MPU-6050 you are using (0x68 is the default).
        """
        self.address = address
        self.i2c = i2c

    def wake(self) -> None:
        """Wake up the MPU-6050."""
        self.i2c.writeto_mem(self.address, 0x6B, bytes([0x01]))

    def sleep(self) -> None:
        """Places MPU-6050 in sleep mode (low power consumption). Stops the internal reading of new data. Any calls to get gyro or accel data while in sleep mode will remain unchanged - the data is not being updated internally within the MPU-6050!"""
        self.i2c.writeto_mem(self.address, 0x6B, bytes([0x40]))

    def who_am_i(self) -> int:
        """Returns the address of the MPU-6050 (ensure it is working)."""
        return self.i2c.readfrom_mem(self.address, 0x75, 1)[0]

    def read_temperature(self) -> float:
        """Reads the temperature, in celsius, of the onboard temperature sensor of the MPU-6050."""
        data = self.i2c.readfrom_mem(self.address, 0x41, 2)
        raw_temp:float = self._translate_pair(data[0], data[1])
        temp:float = (raw_temp / 340.0) + 36.53
        return temp

    def read_gyro_range(self) -> int:
        """Reads the gyroscope range setting."""
        return self._hex_to_index(self.i2c.readfrom_mem(self.address, 0x1B, 1)[0])

    def write_gyro_range(self, range:int) -> None:
        """Sets the gyroscope range setting."""
        self.i2c.writeto_mem(self.address, 0x1B, bytes([self._index_to_hex(range)]))

    def read_gyro_data(self) -> tuple[float, float, float]:
        """Read the gyroscope data, in a (x, y, z) tuple."""

        # set the modified based on the gyro range (need to divide to calculate)
        gr:int = self.read_gyro_range()
        modifier:float = None
        if gr == 0:
            modifier = 131.0
        elif gr == 1:
            modifier = 65.5
        elif gr == 2:
            modifier = 32.8
        elif gr == 3:
            modifier = 16.4

        # read data
        data = self.i2c.readfrom_mem(self.address, 0x43, 6) # read 6 bytes (gyro data)
        x:float = (self._translate_pair(data[0], data[1])) / modifier
        y:float = (self._translate_pair(data[2], data[3])) / modifier
        z:float = (self._translate_pair(data[4], data[5])) / modifier

        return (x, y, z)

    def read_accel_range(self)
        """Reads the accelerometer range setting."""
        return self._hex_to_index(self.i2c.readfrom_mem(self.address, 0x1C, 1)[0])

    def write_accel_range(self, range:int) -> None:
        """Sets the gyro accelerometer setting."""
        self.i2c.writeto_mem(self.address, 0x1C, bytes([self._index_to_hex(range)]))

    def read_accel_data(self) -> tuple[float, float, float]:
        """Read the accelerometer data, in a (x, y, z) tuple."""

        # set the modified based on the gyro range (need to divide to calculate)
        ar:int = self.read_accel_range()
        modifier:float = None
        if ar == 0:
            modifier = 16384.0
        elif ar == 1:
            modifier = 8192.0
        elif ar == 2:
            modifier = 4096.0
        elif ar == 3:
            modifier = 2048.0

        # read data
        data = self.i2c.readfrom_mem(self.address, 0x3B, 6) # read 6 bytes (accel data)
        x:float = (self._translate_pair(data[0], data[1])) / modifier
        y:float = (self._translate_pair(data[2], data[3])) / modifier
        z:float = (self._translate_pair(data[4], data[5])) / modifier

        return (x, y, z)

    def read_lpf_range(self) -> int:
        return self.i2c.readfrom_mem(self.address, 0x1A, 1)[0]

    def write_lpf_range(self, range:int) -> None:
        """
        Sets low pass filter range.
        :param range: Low pass range setting, 0-6. 0 = minimum filter, 6 = maximum filter.
        """

        # check range
        if range < 0 or range > 6:
            raise Exception("Range '" + str(range) + "' is not a valid low pass filter setting.")

        self.i2c.writeto_mem(self.address, 0x1A, bytes([range]))


    #### UTILITY FUNCTIONS BELOW ####

    def _translate_pair(self, high:int, low:int) -> int:
        """Converts a byte pair to a usable value. Borrowed from https://github.com/m-rtijn/mpu6050/blob/0626053a5e1182f4951b78b8326691a9223a5f7d/mpu6050/mpu6050.py#L76C39-L76C39."""
        value = (high << 8) + low
        if value >= 0x8000:
            value = -((65535 - value) + 1)
        return value   

    def _hex_to_index(self, range:int) -> int:
        """Converts a hexadecimal range setting to an integer (index), 0-3. This is used for both the gyroscope and accelerometer ranges."""
        if range== 0x00:
            return 0
        elif range == 0x08:
            return 1
        elif range == 0x10:
            return 2
        elif range == 0x18:
            return 3
        else:
            raise Exception("Found unknown gyro range setting '" + str(range) + "'")

    def _index_to_hex(self, index:int) -> int:
        """Converts an index integer (0-3) to a hexadecimal range setting. This is used for both the gyroscope and accelerometer ranges."""
        if index == 0:
            return 0x00
        elif index == 1:
            return 0x08
        elif index == 2:
            return 0x10
        elif index == 3:
            return 0x18
        else:
            raise Exception("Range index '" + index + "' invalid. Must be 0-3.")

Esp32 boot file

# This file is executed on every boot (including wake-boot from deepsleep)
import esp
esp.osdebug(None)

# webrepl
#import webrepl
#webrepl.start()

# config file
import ujson
with open("settings.json") as f:
    CONFIG = ujson.load(f)

# networking
import network
sta_if = network.WLAN(network.STA_IF)
sta_if.active(True)
sta_if.connect(CONFIG['SSID'], CONFIG['PASSWD'])
while not sta_if.isconnected():
    pass
print('Connected to WiFi:', sta_if.ifconfig())

Config file

{
    "SSID": "some_wifi",
    "PASSWD": "SuperStrongPassword#!0"
}

TODO

Fork mpu6050.py and implement angle calculation method;
Implement a filter to raw data from accelerometer;
Implement a reset to zero function;
Implement a calibration method for the accelerometer;
Implement a function to change the axes orientation;

About

Author

Name: Frederico Sales
link: https://assintotica.xyz
e-mail: frederico@fredericosales.eng.br
slug: GUIMBAL
date: 2024-12-05

References

https://ieeexplore.ieee.org/document/9317254 ↩
https://ieeexplore.ieee.org/document/7375580 ↩
https://www.spiedigitallibrary.org ↩
https://ieeexplore.ieee.org/document/953259 ↩
https://ieeexplore.ieee.org/document/7375580 ↩