I am trying to use am MPU6050 Gyro and accelerationsensor with my pi pico.
It worked just fine on a breadpoard so I soldered it onto a protoboard along with a Ultrasoic sensor, wich doesnt work for some reason, and a 8 switch array.
At first it worked fine but then I got out of 8 of 10 Test the OSError and then I only got that Error.
This is the code I use to read the data and combine the gyro and accelerometer:
Code: Select all
import smbus
import math
import time
from utime import sleep_ms
# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c
gyro_scale = 131.0
accel_scale = 16384.0
address = 0x68 # This is the address value read via the i2cdetect command
def read_all():
raw_gyro_data = bus.read_i2c_block_data(address, 0x43, 6)
raw_accel_data = bus.read_i2c_block_data(address, 0x3b, 6)
gyro_scaled_x = twos_compliment((raw_gyro_data[0] << 8) + raw_gyro_data[1]) / gyro_scale
gyro_scaled_y = twos_compliment((raw_gyro_data[2] << 8) + raw_gyro_data[3]) / gyro_scale
gyro_scaled_z = twos_compliment((raw_gyro_data[4] << 8) + raw_gyro_data[5]) / gyro_scale
accel_scaled_x = twos_compliment((raw_accel_data[0] << 8) + raw_accel_data[1]) / accel_scale
accel_scaled_y = twos_compliment((raw_accel_data[2] << 8) + raw_accel_data[3]) / accel_scale
accel_scaled_z = twos_compliment((raw_accel_data[4] << 8) + raw_accel_data[5]) / accel_scale
return (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z)
def twos_compliment(val):
if (val >= 0x8000):
return -((65535 - val) + 1)
else:
return val
def dist(a, b):
return math.sqrt((a * a) + (b * b))
def get_y_rotation(x,y,z):
radians = math.atan2(x, dist(y,z))
return -math.degrees(radians)
def get_x_rotation(x,y,z):
radians = math.atan2(y, dist(x,z))
return math.degrees(radians)
bus = smbus.SMBus(0) # or bus = smbus.SMBus(1) for Revision 2 boards
sleep_ms(100)
# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)
now = time.time()
K = 0.98
K1 = 1 - K
time_diff = 0.01
(gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = read_all()
last_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
last_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
gyro_offset_x = gyro_scaled_x
gyro_offset_y = gyro_scaled_y
gyro_total_x = (last_x) - gyro_offset_x
gyro_total_y = (last_y) - gyro_offset_y
print("{0:.4f} {1:.2f} {2:.2f} {3:.2f} {4:.2f} {5:.2f} {6:.2f}".format( time.time() - now, (last_x), gyro_total_x, (last_x), (last_y), gyro_total_y, (last_y)))
while True:
for i in range(0, int(3.0 / time_diff)):
time.sleep(time_diff - 0.005)
(gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = read_all()
gyro_scaled_x -= gyro_offset_x
gyro_scaled_y -= gyro_offset_y
gyro_x_delta = (gyro_scaled_x * time_diff)
gyro_y_delta = (gyro_scaled_y * time_diff)
gyro_total_x += gyro_x_delta
gyro_total_y += gyro_y_delta
rotation_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
rotation_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
last_x = K * (last_x + gyro_x_delta) + (K1 * rotation_x)
last_y = K * (last_y + gyro_y_delta) + (K1 * rotation_y)
print(rotation_x)
Code: Select all
""" Provides an SMBus class for use on micropython """
try:
from machine import I2C
except ImportError:
raise ImportError("Can't find the micropython machine.I2C class: "
"perhaps you don't need this adapter?")
class SMBus(I2C):
""" Provides an 'SMBus' module which supports some of the py-smbus
i2c methods, as well as being a subclass of machine.I2C
Hopefully this will allow you to run code that was targeted at
py-smbus unmodified on micropython.
Use it like you would the machine.I2C class:
import usmbus.SMBus
bus = SMBus(1, pins=('G15','G10'), baudrate=100000)
bus.read_byte_data(addr, register)
... etc
"""
def read_byte_data(self, addr, register):
""" Read a single byte from register of device at addr
Returns a single byte """
return self.readfrom_mem(addr, register, 1)[0]
def read_i2c_block_data(self, addr, register, length):
""" Read a block of length from register of device at addr
Returns a bytes object filled with whatever was read """
return self.readfrom_mem(addr, register, length)
def write_byte_data(self, addr, register, data):
""" Write a single byte from buffer `data` to register of device at addr
Returns None """
# writeto_mem() expects something it can treat as a buffer
if isinstance(data, int):
data = bytes([data])
return self.writeto_mem(addr, register, data)
def write_i2c_block_data(self, addr, register, data):
""" Write multiple bytes of data to register of device at addr
Returns None """
# writeto_mem() expects something it can treat as a buffer
if isinstance(data, int):
data = bytes([data])
return self.writeto_mem(addr, register, data)
# The follwing haven't been implemented, but could be.
def read_byte(self, *args, **kwargs):
""" Not yet implemented """
raise RuntimeError("Not yet implemented")
def write_byte(self, *args, **kwargs):
""" Not yet implemented """
raise RuntimeError("Not yet implemented")
def read_word_data(self, *args, **kwargs):
""" Not yet implemented """
raise RuntimeError("Not yet implemented")
def write_word_data(self, *args, **kwargs):
""" Not yet implemented """
raise RuntimeError("Not yet implemented")
Thanks in advance