def dof9_init():
SDA0 = Pin(8) # GPI0 8 is Pin 11
SCL0 = Pin(9) # GPIO 9 is Pin 12
i2c0 = I2C(0, scl=SCL0, sda=SDA0)
dof9 = MPU9250(i2c0)
print("MPU9250 id: {}".format(hex(dof9.whoami)))
return dof9
def __init__(self):
self.lcd = LS027B4DH01(
SPI(2,
baudrate=2_000_000,
polarity=0,
phase=0,
bits=8,
firstbit=SPI.LSB,
sck=Pin(18),
mosi=Pin(23),
miso=Pin(19)), Pin(32, Pin.OUT), Pin(33, Pin.OUT),
Pin(25, Pin.OUT))
self.button = Joystick(Pin(34), Pin(35), Pin(26, Pin.IN))
self.mpu9250 = MPU9250(I2C(scl=Pin(21), sda=Pin(22), freq=100000))
self.mpu9250.setting(self.mpu9250.GFS_1000, self.mpu9250.AFS_16G)
self.states = {
'home': Home(self),
'record': Record(self),
'settings': Settings(self)
}
self.state = 'home'
def unit_test():
print("\n\
from microbit import *\n\
from machine import Pin, I2C\n\
from mpu9250 import MPU9250\n\
from time import sleep\n\
i2c = I2C(scl=Pin(22), sda=Pin(21), freq=200000)\n\
sensor = MPU9250(i2c)\n\
print('MPU9250 whoami: ' + hex(sensor.whoami))\n\
compass = Compass(sensor)\n\
while True:\n\
sleep(0.1)\n\
needle = ((15 - compass.heading()) // 30) % 12\n\
display.show(Image.ALL_CLOCKS[needle])\n\
")
from display import Display, Image
display = Display()
from machine import Pin, I2C
from mpu9250 import MPU9250
from time import sleep
i2c = I2C(scl=Pin(22), sda=Pin(21), freq=200000)
sensor = MPU9250(i2c)
print('MPU9250 whoami: ' + hex(sensor.whoami))
compass = Compass(sensor)
compass.calibrate()
while True:
sleep(0.1)
needle = ((15 - compass.heading()) // 30) % 12
display.show(Image.ALL_CLOCKS[needle])
def __init__(self):
self.lcd = LS027B4DH01(spi=SPI(2,
baudrate=2_000_000,
polarity=0,
phase=0,
bits=8,
firstbit=SPI.LSB,
sck=Pin(18),
mosi=Pin(23),
miso=Pin(19)),
scs=Pin(32, Pin.OUT),
extcomin=Pin(33, Pin.OUT),
disp=Pin(25, Pin.OUT))
self.button = Joystick(x=Pin(34), y=Pin(35), s=Pin(26, Pin.IN))
self.mpu9250 = MPU9250(I2C(scl=Pin(21), sda=Pin(22), freq=100000))
self.mpu9250.setting(self.mpu9250.GFS_1000, self.mpu9250.AFS_16G)
self.states = {
'home': Ui_home(self),
'acceleration': Ui_acceleration(self),
'gyro': Ui_gyro(self),
'geomagnetism': Ui_geomagnetism(self)
}
self.state = 'home'
def setup():
global imu, signe_ax, tensionAlim
wiringpi2.wiringPiSetupGpio() # For GPIO pin numbering
# Initialisation de l'IMU
# initIMU_OK = False
# while not initIMU_OK:
# try:
# imu = MPU9250()
# initIMU_OK = True
# except:
# print("Erreur init IMU")
imu = MPU9250()
# On démarre seulement quand le gyropode dépasse la verticale
# Pour que le gyropode démarre tout seul quand il est sur l'avant
# et si on le redresse quand il est sur l'arrière
signe_ax = -1
CommandeMoteurs(0, 0, tensionAlim)
# Initialisation de la position du servo
a_star.servo(45)
# Mesure de la tension d'alimentation
try:
tensionAlimBrute = a_star.read_battery_millivolts()
tensionAlimAvantMax = tensionAlimBrute / 1000.;
tensionAlim = max(tensionAlimMin, tensionAlimAvantMax);
print "Tension d'alimentation", tensionAlim
except:
print "Probleme lecture tension d'alimentation"
pass
def dof9_calibrate():
SDA0 = Pin(8) # GPI0 8 is Pin 11
SCL0 = Pin(9) # GPIO 9 is Pin 12
i2c0 = I2C(0, scl=SCL0, sda=SDA0)
dummy = MPU9250(i2c) # this opens the bybass to access to the AK8963
ak8963 = AK8963(i2c)
offset, scale = ak8963.calibrate(count=256, delay=200)
def main():
logger.debug("initializing imu...")
imu = MPU9250() # initialize mpu9250, the imu
logger.debug("initializing esc...")
esc_hdd = ESC(config['speed_pin_esc'], config['dir_pin1_esc'],
config['dir_pin2_esc'])
esc_hdd.init_sequence() # perform the esc initialization sequence
# do we want to stream sensor data to the web interface
if config['enable_mission_control']:
logger.info("connecting to mission control server...")
mc.connect() # connect to mission control server
logger.info("starting main loop...")
#acc_array = np.zeros(3,1)
#mag_array = np.zeros(3,1)
while True:
# fetch values from the IMU
imu.update_acc_reading() # update acc values
imu.update_mag_reading() # update mag values
acc_meas_raw = [imu.acc_y * -1, imu.acc_x,
imu.acc_z * -1] # fix imu right hand rule
mag_meas_raw = [imu.mag_x, imu.mag_y * -1,
imu.mag_z] # fix imu right hand rule
# generate the necessary vectors for TRIAD
# TODO don't recreate numpy array every time, heavy memory computation
acc_meas = utils.to_unit_vector(acc_meas_raw) # acc unit vector
mag_meas = utils.to_unit_vector(mag_meas_raw) # mag unit vector
#acc_array[:,0] = acc_meas_raw
#mag_array[:,0] = mag_meas_raw
#acc_meas = numpy.linalg.oth(acc_array)
#mag_meas = numpu.linalg.oth(mag_array)
acc_ref = np.array([0.0, 0.0, -1.0]) # acc ref vector
mag_ref = np.array([1.0, 0.0, 0.0]) # mag ref vector
# make sure none of the vectors are zero vectors
if not (np.any(acc_meas) and np.any(mag_meas)):
logger.warn("measurement zero vector computed")
continue
# compute the rotation matrix with the aforemened vectors
rotation = triad.triad(acc_meas, mag_meas, acc_ref, mag_ref)
logger.debug("acc: {} mag: {} rotation: {}".format(
acc_meas, mag_meas, rotation))
# broadcast all data to mission control server
# this is the real-time gui thing
if config['enable_mission_control']:
mc.broadcast(acc_meas_raw, mag_meas_raw, rotation)
# wait a sec before proceeding, this is our
# update frequency.
time.sleep(0.05)
def __init__(self):
i = 0
for pins in PINS_IMU:
try:
self.vector[i] = MPU9250(pins=pins)
print("Enabling sensor id:{}".format(i))
except Exception as err:
print(err)
i += 1
def main():
logger.debug("Initializing the imu...")
# Initialize the imu.
imu = MPU9250()
# Initialize the speed controller.
logger.debug("initializing esc...")
esc_hdd = ESC(config['speed_pin_esc'], config['dir_pin1_esc'],
config['dir_pin2_esc'])
esc_hdd.init_sequence()
# If enabled, connect to the web interface server.
if config['enable_mission_control']:
logger.info("connecting to mission control server...")
mc.connect() # connect to mission control server
logger.info("starting main loop...")
# acc_array = np.zeros(3,1)
# mag_array = np.zeros(3,1)
while True:
# Fetch and calculate raw measure values from the IMU.
raw_acc_measure, raw_mag_measure = get_updated_sensor_data(imu)
# TODO don't recreate numpy array every time, heavy memory computation
# Generate the necessary vectors for TRIAD.
acc_measure_uvec = utils.to_unit_vector(raw_acc_measure)
mag_measure_uvec = utils.to_unit_vector(raw_mag_measure)
# acc_array[:,0] = acc_meas_raw
# mag_array[:,0] = mag_meas_raw
# acc_meas = numpy.linalg.oth(acc_array)
# mag_meas = numpu.linalg.oth(mag_array)
# Create reference vectors for the accelerometer and magnetometer.
acc_refvec = np.array([0.0, 0.0, -1.0])
mag_refvec = np.array([1.0, 0.0, 0.0])
# Make sure none of the vectors are zero vectors.
if not (np.any(acc_measure_uvec) and np.any(mag_measure_uvec)):
logger.warn("measurement zero vector computed")
continue
# Compute the rotation matrix with the aforementioned vectors.
rotation = triad.triad(acc_measure_uvec, mag_measure_uvec, acc_refvec,
mag_refvec)
logger.debug("acc: {} mag: {} rotation: {}".format(
acc_measure_uvec, mag_measure_uvec, rotation))
# Broadcast the raw measures to the mission control server.
if config['enable_mission_control']:
mc.broadcast(raw_acc_measure, raw_mag_measure, rotation)
# Simulate update frequency through sleeping.
time.sleep(0.05)
def gyro_start(obj):
lcd.image(0, 0, '/flash/img/3-3.jpg', type=lcd.JPG)
global _pos
_pos = [0 , 0]
from mpu9250 import MPU9250
obj['imu'] = MPU9250(i2c)
buffer = []
for i in range(0, 6):
buffer.append([0 , 0])
obj['buf'] = buffer
lcd.rect(65, 65, 60, 60, lcd.WHITE, lcd.WHITE) # old pic dot clean
def calibrateCompass():
if _mpu9250:
i2c = I2C(scl=Pin(22), sda=Pin(21), freq=400000)
print("Calibrating compass: keep turning BPI:BIT for 15 seconds.")
ak8963 = AK8963(i2c)
offset, scale = ak8963.calibrate(count=150, delay=100)
print("Calibration completed.")
print("AK8963 offset:")
print(offset)
print("AK8963 scale:")
print(scale)
_mpu9250 = MPU9250(i2c, ak8963=ak8963)
def gyro_start(obj):
global _pos
_pos = [0, 0]
try:
from mpu6050 import MPU6050
obj['motion'] = MPU6050(i2c, accel_sf=10)
except:
from mpu9250 import MPU9250
obj['motion'] = MPU9250(i2c)
obj['buf'] = [[0, 0] for i in range(0, 6)]
tft.rect(65, 65, 60, 60, tft.WHITE, tft.WHITE) # old pic dot clean
def cthread(out_buf):
imu = MPU9250('X', transposition=(0, 1, 2), scaling=(1, -1, 1))
global i2c_object
i2c_object = imu._mpu_i2c
yield 0.03 # Allow accelerometer to settle
compass = TCCompass(imu, timeout=1000)
while True:
yield
try:
if compass.process():
out_buf.write(compass.output)
except:
out_buf.write(ERR('Compass Error').msg)
def gyro_start(obj):
global _pos
_pos = [0, 0]
lcd.image(0, 0, '/flash/img/3-3.jpg', type=lcd.JPG)
try:
from mpu6050 import MPU6050
obj['imu'] = MPU6050(i2c)
except:
from mpu9250 import MPU9250
obj['imu'] = MPU9250(i2c)
obj['buf'] = [[0, 0] for i in range(0, 6)]
lcd.rect(65, 65, 60, 60, lcd.WHITE, lcd.WHITE) # old pic dot clean
def get_updated_sensor_data(imu: MPU9250) -> tuple:
"""Gets updated raw measures of the acceloremter and magnetometer.
Parameters
----------
x : MPU9250
An instance of the MPU9250 sensor class.
Returns
-------
tuple
A tuple of two vectors, represented by lists. Each list represents a
vector [x, y, z]. The format is (accelerometer, magnetometer).
"""
# Get new readings from the accelerometer and magnetometer.
imu.update_acc_reading()
imu.update_mag_reading()
# Calculate the raw measures by fixing the imu right hand rule.
raw_acc_measure = [imu.acc_y * -1, imu.acc_x, imu.acc_z * -1]
raw_mag_measure = [imu.mag_x, imu.mag_y * -1, imu.mag_z]
# Return a tuple of the calculated values.
return (raw_acc_measure, raw_mag_measure)
def main():
from m5stack import lcd, buttonA
from mpu9250 import MPU9250
from time import sleep_ms
from machine import I2C
import network
# lcd.font(lcd.FONT_Small, transparent=True, fixedwidth=False)
try:
sta_if = network.WLAN(network.STA_IF)
if sta_if.active() == True:
m_acc = network.mqtt('acc', 'mqtt://srdmobilex.ddns.net', port=65530, clientid='a')
m_gyro = network.mqtt('gyro', 'mqtt://srdmobilex.ddns.net', port=65530, clientid='g')
m_acc.start()
m_gyro.start()
m_acc.subscribe('acc', 2)
m_gyro.subscribe('gyro', 0)
i2c = I2C(sda = 21, scl = 22)
imu = MPU9250(i2c)
lcd.clear()
lcd.setColor(lcd.WHITE)
lcd.font(lcd.FONT_Small, transparent=False)
counter = 0
while not buttonA.isPressed():
accel = imu.acceleration
gyro = imu.gyro
mag = imu.magnetic
lcd.print("ACCEL: {:+7.2f} {:+7.2f} {:+7.2f}".format(accel[0], accel[1], accel[2]), lcd.CENTER, 20)
lcd.print("GYRO: {:+7.2f} {:+7.2f} {:+7.2f}".format(gyro[0], gyro[1], gyro[2]), lcd.CENTER, 40)
lcd.print("MAG: {:+7.2f} {:+7.2f} {:+7.2f}".format(mag[0], mag[1], mag[2]), lcd.CENTER, 60)
if sta_if.active() == True:
m_acc.publish('acc', '{:+7.2f},{:+7.2f},{:+7.2f}'.format(accel[0], accel[1], accel[2]) + '(%d)'%counter)
m_gyro.publish('gyro', '{:+7.2f} {:+7.2f} {:+7.2f}'.format(gyro[0], gyro[1], gyro[2]) + '(%d)'%counter)
counter += 1
sleep_ms(20)
m_acc.free()
m_gyro.free()
i2c.deinit()
lcd.print('Exit.', 0, 100)
except:
return -1
def setup():
global imu, signe_ax, tensionAlim
pinMode(directionMoteurDroit, OUTPUT)
pinMode(pwmMoteurDroit, OUTPUT)
pinMode(directionMoteurGauche, OUTPUT)
pinMode(pwmMoteurGauche, OUTPUT)
pinMode(13, OUTPUT)
# Initialisation de l'IMU
# initIMU_OK = False
# while not initIMU_OK:
# try:
# imu = MPU9250()
# initIMU_OK = True
# except:
# print("Erreur init IMU")
imu = MPU9250(i2cbus=2, address=0x69)
# On démarre seulement quand le gyropode dépasse la verticale
# Pour que le gyropode démarre tout seul quand il est sur l'avant
# et si on le redresse quand il est sur l'arrière
signe_ax = -1
CommandeMoteurDroit(0, tensionAlim)
CommandeMoteurGauche(0, tensionAlim)
# Initialisation de la position du servo
uno.servo(45)
# La mesure de la tension d'alimentation se fait via un pont diviseur de rapport 3 / 13
# Les entrées analogiques A0 et A1 sont sur 6 bits avant une plage de variation de 2V
# La résolution obtenue avec la commande suivante est donc très mauvaise
#tensionAlimBrute = analogReadmV(A0)
#tensionAlim = max(tensionAlimMin, float(tensionAlimBrute) * 13. / 3.) / 1000.
# On préfère lire la tension à partir de la mesure faire par la carte Iteaduino Uno
try:
tensionAlimBrute = uno.analog_read(0)
tensionAlimAvantMax = 3.3 * tensionAlimBrute * (13. / 3.) / 1024.;
tensionAlim = max(tensionAlimMin, tensionAlimAvantMax);
print "Tension d'alimentation", tensionAlim
except:
print "Probleme lecture tension d'alimentation"
pass
def __init__(self, i2c, ak8963=None):
# Initialise the sensor
self.sensor = MPU9250(i2c, ak8963=ak8963)
# Initialise the class variables
self.G_roll = 0
self.G_pitch = 0
self.G_yaw = 0
# Offset values for the gyroscope output
self._gyroOffset = (-83.53642,78.94066,-69.39014)
self._gyroScale = (0.1, 0.1, 0.1)
# Initialise the filters for the accelerometer and gyroscope
_a = [1.0000, -1.8879, 0.8946] # Denominator coefficients
# Gain
_K = sum(_a) / 4
_b = [1.0000*_K, 1.9701*_K, 0.9703*_K] # Numerator coefficients
self.accel_filter_x = iir(_a, _b)
self.accel_filter_y = iir(_a, _b)
self.accel_filter_z = iir(_a, _b)
self.mag_filter_x = iir(_a, _b)
self.mag_filter_y = iir(_a, _b)
self.mag_filter_z = iir(_a, _b)
self.gyro_filter_x = iir(_a, _b)
self.gyro_filter_y = iir(_a, _b)
self.gyro_filter_z = iir(_a, _b)
# Initialise Kalman filter matrices
_q = 0.0001 # Process noise
_r = 10 # Signal noise
_Q = matrix.matrix(4, 4, data=[[_q, 0, 0, 0],
[0, _q, 0, 0],
[0, 0, _q, 0],
[0, 0, 0, _q]])
_R = matrix.matrix(4, 4, data=[[_r, 0, 0, 0],
[0, _r, 0, 0],
[0, 0, _r, 0],
[0, 0, 0, _r]])
# Create the Kalman filter
self.kalman_filter = eulerKalman(_Q, _R)
def calMPU():
#Calibració magnetometre
bypass = MPU9250(i2c_gy91)
ak8963 = AK8963(i2c_gy91)
offsetMag, scaleMag = ak8963.calibrate(count=256, delay=200)
#Calibracio Giroscopi
mpu6500 = MPU6500(i2c_gy91)
offsetGyro = mpu6500.calibrate(count=256, delay=0)
#Guardar dades de calibracio
logCal = open('/sd/cal.py', 'w')
logCal.write('offsetMag = {} \nscaleMag = {} \noffsetGyro = {}'.format(
offsetMag, scaleMag, offsetGyro))
logCal.close()
MPUCalibrat = True
return offsetMag, scaleMag, offsetGyro
def unit_test():
print("\n\
")
from display import Display
display = Display()
from machine import Pin, I2C
from mpu9250 import MPU9250
from time import sleep
i2c = I2C(scl=Pin(22), sda=Pin(21), freq=200000)
sensor = MPU9250(i2c)
print('MPU9250 whoami: ' + hex(sensor.whoami))
gs = Direction(sensor)
t = 0
while True:
res = gs.get_direction()
if res != None:
print(res)
t = 1 + t
print(t)
def __init__(self,
i2c,
accel_scale_factor=SF_G,
accel_func_scale=ACCEL_FS_SEL_2G,
gyro_scale_factor=SF_DEG_S,
gyro_func_scale=GYRO_FS_SEL_250DPS,
offset_default=(0, 0, 0),
scale_default=(1, 1, 1)):
""" SF scale factor -> facteur de mise à l'échelle (unites)
accel_scale_factor :
SF_G = 1
SF_M_S2 = 9.80665 # 1 g = 9.80665 m/s2 ie. standard gravity
gyro_scale_factor :
SF_DEG_S = 1
SF_RAD_S = 0.017453292519943 # 1 deg/s is 0.017453292519943 rad/s
FS functionning scale : plage de mesure
gyro_func_scale :
ACCEL_FS_SEL_2G
ACCEL_FS_SEL_4G
ACCEL_FS_SEL_8G
ACCEL_FS_SEL_16G
gyro_func_scale (deg/s):
GYRO_FS_SEL_250DPS
GYRO_FS_SEL_500DPS
GYRO_FS_SEL_1000DPS
GYRO_FS_SEL_2000DPS
offset_default, offset_default : obtenu par la calibration
"""
self.mpu6500 = MPU6500(i2c,
accel_sf=accel_scale_factor,
accel_fs=accel_func_scale,
gyro_sf=gyro_scale_factor,
gyro_fs=gyro_func_scale)
self.ak8963 = AK8963(i2c, offset=offset_default, scale=scale_default)
#self.ak8963 = AK8963(i2c)
sensor = MPU9250(i2c, mpu6500=self.mpu6500, ak8963=self.ak8963)
print("MPU9250 id: " + hex(sensor.whoami))
def init():
global uart
# Initialising UART6 (Y1, Y2)
uart = UART(6, baudrate=38400, timeout=10, read_buf_len=200)
pyb.repl_uart(uart)
print('RPi UART setup')
global uart_gps
global gps_device
global gps_planner
uart_gps = UART(4, baudrate=9600, timeout=10, read_buf_len=600)
gps_device = gps.GPS(uart_gps)
gps_planner = planner.PLANNER(gps_device)
print("GPS set up")
global pid_test
pid_test = pid.PID(0.3, 0, 0.1, gps_planner.dist_bearing, 1)
# If True, uart messages are sent back normally,
# if false, only log messages are sent back.
global UART_comms_master
UART_comms_master = False
global imu, fuse, i2c
global imu_acc, imu_gyr, imu_mag
i2c = I2C(scl=Pin('Y9'), sda=Pin('Y10'))
try:
imu = MPU9250(i2c, scaling=(-1, -1, -1))
imu._mag.cal = (20.915039062, 11.880468750, 23.293359375)
imu.gyro_filter_range = 4
imu.accel_filter_range = 4
print(imu.mag.cal)
print(imu.accel.cal)
print(imu.gyro.cal)
mpu_addr = "MPU9250 id: {:X}".format(imu.mpu_addr)
cmd.send_uart(mpu_addr, True)
fuse = Fusion()
except:
cmd.send_uart("No imu detected", True)
sensors.ina_init(i2c)
def __init__(self):
self.fortunelist = ["大吉", "中吉", "小吉", "末吉", "吉"]
self.menulist = [
"肉", "魚", "定食", "丼", "米", "中華風", "和風", "洋風", "イタリアン", "エスニック",
"麺類", "カレー味", "ビル出て右", "ビル出て左", "赤いもの", "白いもの", "茶色いもの", "黄色いもの",
"温かいもの", "女子力高いもの", "おっさん飯", "居酒屋めし", "からいもの", "とろみがある", "ビル1F",
"2F以上", "ビル地下", "ファミレス", "スプーンで食う", "おばちゃん店員", "900円以内"
]
self.challengedict = {
0: {
"方法": "ダイスで決めようぜ!",
"選択肢": ["偶数", "奇数"]
},
1: {
"方法": "近くの人とジャンケン",
"選択肢": ["勝ち", "負け"]
},
2: {
"方法": "AかつB",
"選択肢": ["A", "B"]
}
}
self.jpfontObj = jpfont()
#いらすとや再頒布NGのため、いらすとやで「ご飯」を検索してDLしてください。pngでは表示されません。
self.backimg = "/flash/assets/food_gohan.jpg"
self.i2c = I2C(sda=21, scl=22)
self.sensor = MPU9250(self.i2c)
lcd.clear()
lcd.image(0, 0, self.backimg)
lcd.setCursor(0, 0)
lcd.setColor(lcd.BLACK)
buttonA.wasPressed(self.on_AwasPressed)
buttonB.wasPressed(self.on_BwasPressed)
buttonC.wasPressed(self.on_CwasPressed)
def unit_test():
print("\n\
")
from display import Display
display = Display()
from machine import Pin, I2C
from mpu9250 import MPU9250
from time import sleep
i2c = I2C(scl=Pin(22), sda=Pin(21), freq=200000)
sensor = MPU9250(i2c)
print('MPU9250 whoami: ' + hex(sensor.whoami))
accelerometer = Accelerometer(sensor)
while True:
sleep(0.1)
reading = accelerometer.get_x()
print(reading)
print(accelerometer.get_values())
if reading > 20:
display.show("R")
elif reading < -20:
display.show("L")
else:
display.show("-")
def senMPU():
global accel, acX, acY, acZ, gyro, gX, gY, gZ, mag, mX, mY, mZ
bypass = MPU9250(i2c_gy91)
if MPUCalibrat:
ak8963 = AK8963(i2c_gy91, offset=offsetMag, scale=scaleMag)
#Pendent d'actualització de la llibreria a la versio 4.0
#mpu6500 = MPU6500(i2c_gy91, offset=offsetGyro, gyro_sf=SF_DEG_S)
mpu6500 = MPU6500(i2c_gy91, gyro_sf=SF_DEG_S)
else:
ak8963 = AK8963(i2c_gy91)
mpu6500 = MPU6500(i2c_gy91, gyro_sf=SF_DEG_S)
mpu = mpu9250.MPU9250(i2c_gy91, ak8963=ak8963, mpu6500=mpu6500)
#Reorientacio dels eixos del accelerometre i el giroscopi per a que tinguin la mateixa disposicio que el magnetometre
#Veure eixos al datasheet MPU9250
accel, gyro = orientate((1, 0, 2), (False, False, True), mpu.acceleration,
mpu.gyro)
acX, acY, acZ = accel
gX, gY, gZ = gyro
mag = mpu.magnetic
mX, mY, mZ = mag
import utime
from random import randint
from machine import I2C, Pin
from mpu9250 import MPU9250
i2c = I2C(scl=Pin(22), sda=Pin(21), freq=400000)
sensor = MPU9250(i2c)
print("MPU9250 id: " + hex(sensor.whoami))
from Graph import BpiBitNeoPixel, NeoPixelPower
NeoPixelPower(True)
View = BpiBitNeoPixel()
X, Y, Color, Flag = 2, 2, 2, 0
while Thread[0]:
# print('acceleration:', sensor.acceleration)
# print('gyro:', sensor.gyro)
# print('magnetic:', sensor.magnetic)
A = sensor.acceleration # -1 and -2 Software correction
View.LoadXY(X, Y, (0, 0, 0))
if (A[1] > -1 and A[1] > X and X < View.Max - 1):
X = X + 1
elif (A[1] < -1 and A[1] < X and X > View.Min):
X = X - 1
if (A[0] > -2 and A[0] > Y and Y > View.Min):
Y = Y - 1
elif (A[0] < -2 and A[0] < Y and Y < View.Max - 1):
Y = Y + 1
Color = Color + Flag
if (Color == 10): Flag = -2
elif (Color == 2): Flag = +2
View.LoadXY(X, Y, (0, Color, Color))
# fusiontest.py Simple test program for sensor fusion on Pyboard
# Author Peter Hinch
# Released under the MIT License (MIT)
# Copyright (c) 2017 Peter Hinch
# V0.8 14th May 2017 Option for external switch for cal test. Make platform independent.
# V0.7 25th June 2015 Adapted for new MPU9x50 interface
from m5stack import lcd, buttonA, buttonB
from machine import Pin
import utime as time
from mpu9250 import MPU9250
from fusion import Fusion
from machine import I2C
i2c = I2C(sda = 21, scl = 22)
imu = MPU9250(i2c)
fuse = Fusion()
def getmag(): # Return (x, y, z) tuple (blocking read)
return imu.mag.xyz
if buttonA.isPressed():
print("Calibrating. Press switch when done.")
fuse.calibrate(getmag, BtnB.press, lambda : time.sleep(0.1))
print(fuse.magbias)
if False:
mag = imu.magnetic # Don't include blocking read in time
accel = imu.acceleration # or i2c
gyro = imu.gyro
start = time.ticks_us() # Measure computation time only
import display
Image = display.Image
display = display.Display()
import button
button_a = button.Button(35)
button_b = button.Button(27)
import temperature
__adc = machine.ADC(machine.Pin(34, machine.Pin.IN))
__adc.atten(machine.ADC.ATTN_11DB)
temperature = temperature.Temperature(__adc).temperature
try:
from mpu9250 import MPU9250
from mpu6500 import MPU6500
__i2c = machine.I2C(scl=machine.Pin(22), sda=machine.Pin(21), freq=200000)
__dev = __i2c.scan()
# print("dev ", __dev)
if 104 in __dev:
print("1.4 version")
__sensor = MPU9250(__i2c, MPU6500(__i2c, 0x68))
if 105 in __dev:
print("1.2 version No compass")
__sensor = MPU9250(__i2c, MPU6500(__i2c, 0x69))
import accelerometer
accelerometer = accelerometer.Direction(__sensor)
import compass
compass = compass.Compass(__sensor)
except Exception as e:
print("MPU9250 Error", e)
from mpu9250 import MPU9250
imu = MPU9250('X', 0)
print(imu.accel.xyz)
print(imu.gyro.xyz)
print(imu.mag.xyz)
print(imu.temperature)
print(imu.accel.z)
# Note there will be small differences between the lines because
# of drift or movement occurring between the readings
from machine import Timer
import time
from mpu9250 import MPU9250
import micropython
micropython.alloc_emergency_exception_buf(100)
def cb(timer): # Callback: populate array members
imu.get_gyro_irq()
imu.get_accel_irq()
imu.get_mag_irq()
tim = Timer.Alarm(cb, 400, periodic=True)
imu = MPU9250()
print("You should see slightly different values on each pair of readings.")
print(" Accelerometer Gyro Magnetometer")
for count in range(50):
time.sleep_ms(400) # Ensure an interrupt occurs to re-populate integer values
scale = 6.6666 # Correction factors involve floating point
mag = list(map(lambda x, y : x*y/scale, imu.mag.ixyz, imu.mag_correction))
print("Interrupt:", [x/16384 for x in imu.accel.ixyz], [x/131 for x in imu.gyro.ixyz], mag)
time.sleep_ms(100)
print("Normal: ", imu.accel.xyz, imu.gyro.xyz, imu.mag.xyz)
print()
tim.cancel()
