Marcus TarquinioCédric Lebocq
Published © MIT

Fun with ATOM Matrix

Simple MicroPython code for the ATOM 5 x 5 LED matrix and its built-in MPU6886 accelerometer. It works with the M5StickC + NeoFlash Hat.

IntermediateWork in progress2 hours8,222

Things used in this project

Hardware components

M5Stack ATOM Matrix
https://things.lu/product/atom-matrix-esp32-development-kit/
×1
M5StickC ESP32-PICO Mini IoT Development Board
M5Stack M5StickC ESP32-PICO Mini IoT Development Board
https://things.lu/product/m5stickc-development-kit-with-hat/
×1
M5StickC NeoFlash Hat
M5Stack M5StickC NeoFlash Hat
https://things.lu/product/m5stickc-neoflash-hat/
×1

Software apps and online services

MicroPython
MicroPython

Story

Read more

Code

mpu6886.py

MicroPython
mpu6886 library
# C.LEBOCQ 02/2020
# MicroPython library for the MPU6886 imu ( M5StickC / ATOM Matrix ) 
# Based on https://github.com/m5stack/M5StickC/blob/master/src/utility/MPU6886.cpp

from machine import I2C
from time import sleep

MPU6886_ADDRESS           = const(0x68)
MPU6886_WHOAMI            = const(0x75)
MPU6886_ACCEL_INTEL_CTRL  = const(0x69)
MPU6886_SMPLRT_DIV        = const(0x19)
MPU6886_INT_PIN_CFG       = const(0x37)
MPU6886_INT_ENABLE        = const(0x38)
MPU6886_ACCEL_XOUT_H      = const(0x3B)
MPU6886_ACCEL_XOUT_L      = const(0x3C)
MPU6886_ACCEL_YOUT_H      = const(0x3D)
MPU6886_ACCEL_YOUT_L      = const(0x3E)
MPU6886_ACCEL_ZOUT_H      = const(0x3F)
MPU6886_ACCEL_ZOUT_L      = const(0x40)

MPU6886_TEMP_OUT_H        = const(0x41)
MPU6886_TEMP_OUT_L        = const(0x42)

MPU6886_GYRO_XOUT_H       = const(0x43)
MPU6886_GYRO_XOUT_L       = const(0x44)
MPU6886_GYRO_YOUT_H       = const(0x45)
MPU6886_GYRO_YOUT_L       = const(0x46)
MPU6886_GYRO_ZOUT_H       = const(0x47)
MPU6886_GYRO_ZOUT_L       = const(0x48)

MPU6886_USER_CTRL         = const(0x6A)
MPU6886_PWR_MGMT_1        = const(0x6B)
MPU6886_PWR_MGMT_2        = const(0x6C)
MPU6886_CONFIG            = const(0x1A)
MPU6886_GYRO_CONFIG       = const(0x1B)
MPU6886_ACCEL_CONFIG      = const(0x1C)
MPU6886_ACCEL_CONFIG2     = const(0x1D)
MPU6886_FIFO_EN           = const(0x23)

#consts for Acceleration & Resolution scale
AFS_2G      = const(0x00)
AFS_4G      = const(0x01)
AFS_8G      = const(0x02)
AFS_16G     = const(0x03)

GFS_250DPS  = const(0x00)
GFS_500DPS  = const(0x01)
GFS_1000DPS = const(0x02)
GFS_2000DPS = const(0x03)

class MPU6886():

    def __init__(self, i2c, Gscale = GFS_2000DPS, Ascale = AFS_8G):
        self.i2c = i2c
        self.Gscale = Gscale
        self.Ascale = Ascale
        if self.init():
            self.setAccelFsr(Ascale)
            self.setGyroFsr(Gscale)

    # sleep in ms
    def sleepms(self,n):
        sleep(n / 1000)
    
    # set I2C reg (1 byte)
    def	setReg(self, reg, dat):
        self.i2c.writeto(MPU6886_ADDRESS, bytearray([reg, dat]))
		
    # get I2C reg (1 byte)
    def	getReg(self, reg):
        self.i2c.writeto(MPU6886_ADDRESS, bytearray([reg]))
        t =	self.i2c.readfrom(MPU6886_ADDRESS, 1)
        return t[0]

    # get n reg
    def	getnReg(self, reg, n):
        self.i2c.writeto(MPU6886_ADDRESS, bytearray([reg]))
        t =	self.i2c.readfrom(MPU6886_ADDRESS, n)
        return t    

    def init(self):
        tempdata = self.getReg(MPU6886_WHOAMI)
        if tempdata != 0x19:
            return False
        self.sleepms(1)
        regdata = 0x00
        self.setReg(MPU6886_PWR_MGMT_1, regdata)
        self.sleepms(10)      
        regdata = (0x01<<7)
        self.setReg(MPU6886_PWR_MGMT_1, regdata)
        self.sleepms(10)
        regdata = (0x01<<0)
        self.setReg(MPU6886_PWR_MGMT_1, regdata)
        self.sleepms(10)
        regdata = 0x10
        self.setReg(MPU6886_ACCEL_CONFIG, regdata)
        self.sleepms(1)
        regdata = 0x18
        self.setReg(MPU6886_GYRO_CONFIG, regdata)
        self.sleepms(1)
        regdata = 0x01
        self.setReg(MPU6886_CONFIG, regdata)
        self.sleepms(1)
        regdata = 0x05
        self.setReg(MPU6886_SMPLRT_DIV, regdata)
        self.sleepms(1)
        regdata = 0x00
        self.setReg(MPU6886_INT_ENABLE, regdata)
        self.sleepms(1)
        regdata = 0x00
        self.setReg(MPU6886_ACCEL_CONFIG2, regdata)
        self.sleepms(1)
        regdata = 0x00
        self.setReg(MPU6886_USER_CTRL, regdata)
        self.sleepms(1)
        regdata = 0x00
        self.setReg(MPU6886_FIFO_EN, regdata)
        self.sleepms(1)
        regdata = 0x22
        self.setReg(MPU6886_INT_PIN_CFG, regdata)
        self.sleepms(1)
        regdata = 0x01
        self.setReg(MPU6886_INT_ENABLE, regdata)
        self.sleepms(100)
        self.getGres()
        self.getAres()
        return True      

    def getGres(self):
        if self.Gscale == GFS_250DPS:
            self.gRes = 250.0 / 32768.0
        elif self.Gscale == GFS_500DPS:
            self.gRes = 500.0/32768.0
        elif self.Gscale == GFS_1000DPS:
            self.gRes = 1000.0/32768.0
        elif self.Gscale == GFS_2000DPS:
            self.gRes = 2000.0/32768.0
        else:
            self.gRes = 250.0/32768.0

    def getAres(self):
        if self.Ascale == AFS_2G:
            self.aRes = 2.0/32768.0
        elif self.Ascale == AFS_4G:
            self.aRes = 4.0/32768.0
        elif self.Ascale == AFS_8G: 
            self.aRes = 8.0/32768.0
        elif self.Ascale == AFS_16G:
            self.aRes = 16.0/32768.0
        else:
            self.aRes = 2.0/32768.0

    def getAccelAdc(self):
        buf = self.getnReg(MPU6886_ACCEL_XOUT_H,6)
                   
        ax = (buf[0]<<8) | buf[1]
        ay = (buf[2]<<8) | buf[3]
        az = (buf[4]<<8) | buf[5]
        return ax,ay,az

    def getAccelData(self):
        ax,ay,az = self.getAccelAdc()
        if ax > 32768:
            ax -= 65536
        if ay > 32768:
            ay -= 65536
        if az > 32768:
            az -= 65536
        ax *=  self.aRes
        ay *=  self.aRes
        az *=  self.aRes
        return ax,ay,az

    def getGyroAdc(self):
        buf = self.getnReg(MPU6886_GYRO_XOUT_H,6)
        gx = (buf[0]<<8) | buf[1]  
        gy = (buf[2]<<8) | buf[3]  
        gz = (buf[4]<<8) | buf[5]
        return gx,gy,gz

    def getGyroData(self):
        gx,gy,gz = self.getGyroAdc()
        if gx > 32768:
            gx -= 65536
        if gy > 32768:
            gy -= 65536
        if gz > 32768:
            gz -= 65536 
        gx *= self.gRes
        gy *= self.gRes
        gz *= self.gRes
        return gx, gy, gz 

    def getTempAdc(self):
        buf = self.getnReg(MPU6886_TEMP_OUT_H,2)
        return (buf[0]<<8) | buf[1]  

    def getTempData(self):
        return self.getTempAdc() / 326.8 + 25.0

    def setGyroFsr(self,scale):
        regdata = (scale<<3)
        self.setReg(MPU6886_GYRO_CONFIG, regdata)
        self.sleepms(10)
        self.Gscale = scale
        self.getGres()

    def setAccelFsr(self,scale):
        regdata = (scale<<3)
        self.setReg(MPU6886_ACCEL_CONFIG, regdata)
        self.sleepms(10)
        self.Ascale = scale
        self.getAres()

main.py

MicroPython
from mpu6886 import MPU6886
from neopixel import NeoPixel
from machine import Pin, SoftI2C
from time import sleep
from math import *

# Definitions for the ATOM Matrix
LED_GPIO = const(27)
MPU6886_SCL = const(21)
MPU6886_SDA = const(25)
matrix_size_x = const(5)
matrix_size_y = const(5)
is_atom_matrix = True

# Definitions for the M5StickC + NeoFlash hat
#LED_GPIO = const(26)
#MPU6886_SCL = const(22)
#MPU6886_SDA = const(21)
#matrix_size_x = const(18)
#matrix_size_y = const(7)
#is_atom_matrix = False

threshold = const(5) # minimum angle that triggers movement 
color = (0, 0, 20) # Initial color: Blue
x = int(matrix_size_x / 2) # Get matrix
y = int(matrix_size_y / 2) # center
np = NeoPixel(Pin(LED_GPIO), matrix_size_x * matrix_size_y)

def computeAngles(ax,ay,az):
    # https://roboticsclubiitk.github.io/2017/12/21/Beginners-Guide-to-IMU.html
    pitch = 180 * atan (ax/sqrt(ay**2 + az**2))/ pi
    roll = 180 * atan (ay/sqrt(ax**2 + az**2))/ pi
    yaw = 180 * atan (az/sqrt(ax**2 + ay**2))/ pi
    return pitch, roll, yaw

def updateDot(p, angle, size, threshold, color1, color2):
    global color
    if angle > threshold:     # Test if angle is positive
        if p < size - 1:      # If it is not at the matrix
            p = p + 1         # border, move the dot
        else:
            color = color1    # change color if reached the border
    elif angle < - threshold: # Test if angle is negative
        if p > 0:             # If it is not at the matrix
            p = p - 1         # border, move the dot
        else:
            color = color2    # change color if reached the border
    return p

# I2C bus init for MPU6886
i2c = SoftI2C(scl=Pin(MPU6886_SCL), sda=Pin(MPU6886_SDA))

# Values you can use to initialize the accelerometer. AFS_16G, means +-8G sensitivity, and so on
# Larger scale means less precision
AFS_2G      = const(0x00)
AFS_4G      = const(0x01)
AFS_8G      = const(0x02)
AFS_16G     = const(0x03)

# Values you can use to initialize the gyroscope. GFS_2000DPS means 2000 degrees per second sensitivity, and so on
# Larger scale means less precision
GFS_250DPS  = const(0x00)
GFS_500DPS  = const(0x01)
GFS_1000DPS = const(0x02)
GFS_2000DPS = const(0x03)  

# by default, if you initialize MPU6886 with  imu = MPU6886(i2c), GFS_2000DPS and AFS_8G are used
# if you want to initialize with other values you have too use :
# imu = MPU6886(i2c,mpu6886.GFS_250DPS,mpu6886.AFS_4G )
# imu = MPU6886(i2c) #=> use default 8G / 2000DPS
imu = MPU6886(i2c, GFS_500DPS, AFS_4G)

# in order to calibrate Gyroscope you have to put the device on a flat surface
# preferably level with the floor and not touch it during the procedure. (1s for 20 cycles)
#calibrateGyro(20)

while True:
    ax,ay,az = imu.getAccelData()
#    gx,gy,gz = imu.getGyroData()
    pitch, roll, yaw = computeAngles(ax,ay,az)
#    print(ax,ay,az)
#    print(pitch, roll, yaw)
    np[ y * matrix_size_x + x ] = (0,0,0) # Turn off the LED on the current dot possition
    if is_atom_matrix:
        x = updateDot(x, pitch, matrix_size_x, threshold, (20, 0, 0), (20, 20, 0))
        y = updateDot(y, roll, matrix_size_y, threshold, (20, 0, 20), (0, 20, 20))
    else:  # Compensate for the different orientation of the sensor on the M5StickC
        x = updateDot(x, -roll, matrix_size_x, threshold, (20, 0, 0), (20, 20, 0))
        y = updateDot(y, pitch, matrix_size_y, threshold, (20, 0, 20), (0, 20, 20))
    np[ y * matrix_size_x + x ] = color # Turn on the LED on the new dot possition
    np.write()
    sleep(0.2)

Credits

Marcus Tarquinio

Marcus Tarquinio

1 project • 1 follower
Cédric Lebocq

Cédric Lebocq

1 project • 1 follower

Comments

Add projectSign up / Login