From 7ad1490b9fa7e017ad2eeee3d0fa21d84d4836bb Mon Sep 17 00:00:00 2001 From: "karl.hudgell" <karl.hudgell@bjss.com> Date: Fri, 5 Mar 2021 17:18:18 +0000 Subject: [PATCH] initial sample --- boot.py | 5 ++ main.py | 107 +++++++++++++++++++++++++++ mpu6886.py | 213 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 325 insertions(+) create mode 100644 boot.py create mode 100644 main.py create mode 100644 mpu6886.py diff --git a/boot.py b/boot.py new file mode 100644 index 0000000..f50ed9b --- /dev/null +++ b/boot.py @@ -0,0 +1,5 @@ +# This file is executed on every boot (including wake-boot from deepsleep) +#import esp +#esp.osdebug(None) +#import webrepl +#webrepl.start() diff --git a/main.py b/main.py new file mode 100644 index 0000000..7532db5 --- /dev/null +++ b/main.py @@ -0,0 +1,107 @@ +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) \ No newline at end of file diff --git a/mpu6886.py b/mpu6886.py new file mode 100644 index 0000000..aaff90c --- /dev/null +++ b/mpu6886.py @@ -0,0 +1,213 @@ +# 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() \ No newline at end of file