ledTilt.py

   1 from mpu6886 import MPU6886
   2 from neopixel import NeoPixel
   3 from machine import Pin, SoftI2C
   4 from time import sleep
   5 from math import *
   6
   7 # Definitions for the ATOM Matrix
   8 LED_GPIO = const(27)
   9 MPU6886_SCL = const(21)
  10 MPU6886_SDA = const(25)
  11 matrix_size_x = const(5)
  12 matrix_size_y = const(5)
  13 is_atom_matrix = True
  14
  15 # Definitions for the M5StickC + NeoFlash hat
  16 #LED_GPIO = const(26)
  17 #MPU6886_SCL = const(22)
  18 #MPU6886_SDA = const(21)
  19 #matrix_size_x = const(18)
  20 #matrix_size_y = const(7)
  21 #is_atom_matrix = False
  22
  23 threshold = const(5) # minimum angle that triggers movement
  24 color = (0, 0, 20) # Initial color: Blue
  25 x = int(matrix_size_x / 2) # Get matrix
  26 y = int(matrix_size_y / 2) # center
  27 np = NeoPixel(Pin(LED_GPIO), matrix_size_x * matrix_size_y)
  28
  29 def computeAngles(ax,ay,az):
  30     # https://roboticsclubiitk.github.io/2017/12/21/Beginners-Guide-to-IMU.html
  31     pitch = 180 * atan (ax/sqrt(ay**2 + az**2))/ pi
  32     roll = 180 * atan (ay/sqrt(ax**2 + az**2))/ pi
  33     yaw = 180 * atan (az/sqrt(ax**2 + ay**2))/ pi
  34     return pitch, roll, yaw
  35
  36 def updateDot(p, angle, size, threshold, color1, color2):
  37     global color
  38     if angle > threshold:     # Test if angle is positive
  39         if p < size - 1:      # If it is not at the matrix
  40             p = p + 1         # border, move the dot
  41         else:
  42             color = color1    # change color if reached the border
  43     elif angle < - threshold: # Test if angle is negative
  44         if p > 0:             # If it is not at the matrix
  45             p = p - 1         # border, move the dot
  46         else:
  47             color = color2    # change color if reached the border
  48     return p
  49
  50 # I2C bus init for MPU6886
  51 i2c = SoftI2C(scl=Pin(MPU6886_SCL), sda=Pin(MPU6886_SDA))
  52
  53 # Values you can use to initialize the accelerometer. AFS_16G, means +-8G sensitivity, and so on
  54 # Larger scale means less precision
  55 AFS_2G      = const(0x00)
  56 AFS_4G      = const(0x01)
  57 AFS_8G      = const(0x02)
  58 AFS_16G     = const(0x03)
  59
  60 # Values you can use to initialize the gyroscope. GFS_2000DPS means 2000 degrees per second sensitivity, and so on
  61 # Larger scale means less precision
  62 GFS_250DPS  = const(0x00)
  63 GFS_500DPS  = const(0x01)
  64 GFS_1000DPS = const(0x02)
  65 GFS_2000DPS = const(0x03)
  66
  67 # by default, if you initialize MPU6886 with  imu = MPU6886(i2c), GFS_2000DPS and AFS_8G are used
  68 # if you want to initialize with other values you have too use :
  69 # imu = MPU6886(i2c,mpu6886.GFS_250DPS,mpu6886.AFS_4G )
  70 # imu = MPU6886(i2c) #=> use default 8G / 2000DPS
  71 imu = MPU6886(i2c, GFS_500DPS, AFS_4G)
  72
  73 # in order to calibrate Gyroscope you have to put the device on a flat surface
  74 # preferably level with the floor and not touch it during the procedure. (1s for 20 cycles)
  75 #calibrateGyro(20)
  76
  77 while True:
  78     ax,ay,az = imu.getAccelData()
  79 #    gx,gy,gz = imu.getGyroData()
  80     pitch, roll, yaw = computeAngles(ax,ay,az)
  81 #    print(ax,ay,az)
  82 #    print(pitch, roll, yaw)
  83     np[ y * matrix_size_x + x ] = (0,0,0) # Turn off the LED on the current dot possition
  84     if is_atom_matrix:
  85         x = updateDot(x, pitch, matrix_size_x, threshold, (20, 0, 0), (20, 20, 0))
  86         y = updateDot(y, roll, matrix_size_y, threshold, (20, 0, 20), (0, 20, 20))
  87     else:  # Compensate for the different orientation of the sensor on the M5StickC
  88         x = updateDot(x, -roll, matrix_size_x, threshold, (20, 0, 0), (20, 20, 0))
  89         y = updateDot(y, pitch, matrix_size_y, threshold, (20, 0, 20), (0, 20, 20))
  90     np[ y * matrix_size_x + x ] = color # Turn on the LED on the new dot possition
  91     np.write()
  92     sleep(0.2)