initial sample
This commit is contained in:
karl.hudgell
parent
e2d642b425
commit
7ad1490b9f
5
boot.py
Normal file
5
boot.py
Normal file
@ -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()
|
||||
107
main.py
Normal file
107
main.py
Normal file
@ -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)
|
||||
213
mpu6886.py
Normal file
213
mpu6886.py
Normal file
@ -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()
|
||||
Loading…
x
Reference in New Issue
Block a user