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,742

Things used in this project

Hardware components

M5Stack ATOM Matrix
×1
M5StickC ESP32-PICO Mini IoT Development Board
M5Stack M5StickC ESP32-PICO Mini IoT Development Board
Alternatively
×1
M5StickC NeoFlash Hat
M5Stack M5StickC NeoFlash Hat
Alternatively
×1

Software apps and online services

MicroPython
MicroPython

Story

Read more

Code

main.py

MicroPython
main code
from mpu6886 import MPU6886
from neopixel import NeoPixel
from machine import Pin, I2C
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 = 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 = False

threshold = const(5)
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)

avg_gx,avg_gy,avg_gz = 0,0,0

def calibrateGyro(n):
    global avg_gx,avg_gy,avg_gz
    for x in range(0,n):
        gx,gy,gz = imu.getGyroData()
        avg_gx += gx
        avg_gy += gy
        avg_gz += gz
        sleep(0.05)
    avg_gx /= n
    avg_gy /= n
    avg_gz /= n
    
def computeAngles(ax,ay,az):
    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, acel, size, threshold, color1, color2):
    global color
    if acel > threshold:      # Test if acel 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 acel < - threshold:  # Test if acel 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 ATOM Matrix MPU6886
i2c = I2C(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)
#   Use correction for gyroscope by subtracting average data from calibration
#    print(gx-avg_gx,gy-avg_gy,gz-avg_gz)
#    print(pitch, roll, yaw)
    np[ y * matrix_size_x + x ] = (0,0,0) # Turn LED off
    if is_atom:
        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:
        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 LED on
    np.write()
    sleep(0.1)

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()

Credits

Marcus Tarquinio

Marcus Tarquinio

1 project • 1 follower
Cédric Lebocq

Cédric Lebocq

1 project • 1 follower

Comments

Add projectSign up / Login