我正在尝试完成我的大学项目,在 python 中构建和编程自平衡机器人(使用 RPi),但在我的知识中遇到了障碍。
从 MPU6050 我能够获得加速度和陀螺仪数据,此外还有它们的角度(使用 atan2 函数获取加速角度,并使用 dt 积分获取陀螺仪角度)。但是我想不出一种方法来编码这些数据,以便在给定时间(dt)的循环中它将角度的变化(来自陀螺仪数据)转换为总经过角度。
我需要能够循环程序,以便它运行时间 (dt),存储此数据,再次运行程序但期望 + 或 - 先前存储的数据。 然后我将能够使用陀螺仪和加速数据的两个角度,并通过免费过滤器运行它。
我已经上传了下面的相关代码。
import smbus #import SMBus module of I2C
import time
from time import sleep
import math
#some MPU6050 Registers and their Address
PWR_MGMT_1 = 0x6B
SMPLRT_DIV = 0x19
CONFIG = 0x1A
GYRO_CONFIG = 0x1B
INT_ENABLE = 0x38
ACCEL_XOUT_H = 0x3B
ACCEL_YOUT_H = 0x3D
ACCEL_ZOUT_H = 0x3F
GYRO_XOUT_H = 0x43
GYRO_YOUT_H = 0x45
GYRO_ZOUT_H = 0x47
DT = 0.02
rad_to_deg = 180/(math.pi)
def MPU_Init():
#write to sample rate register
bus.write_byte_data(Device_Address, SMPLRT_DIV, 7)
#Write to power management register
bus.write_byte_data(Device_Address, PWR_MGMT_1, 1)
#Write to Configuration register
bus.write_byte_data(Device_Address, CONFIG, 0)
#Write to Gyro configuration register
bus.write_byte_data(Device_Address, GYRO_CONFIG, 24)
#Write to interrupt enable register
bus.write_byte_data(Device_Address, INT_ENABLE, 1)
def read_raw_data(addr):
#Accelero and Gyro value are 16-bit
high = bus.read_byte_data(Device_Address, addr)
low = bus.read_byte_data(Device_Address, addr+1)
#concatenate higher and lower value
value = ((high << 8) | low)
#to get signed value from mpu6050
if(value > 32768):
value = value - 65536
return value
bus = smbus.SMBus(1) # or bus = smbus.SMBus(0) for older version boards
Device_Address = 0x68 # MPU6050 device address
MPU_Init()
print (" Reading Data of Gyroscope and Accelerometer")
while True:
#Read Accelerometer raw value
acc_x = read_raw_data(ACCEL_XOUT_H)
acc_y = read_raw_data(ACCEL_YOUT_H)
acc_z = read_raw_data(ACCEL_ZOUT_H)
#Read Gyroscope raw value
gyro_x = read_raw_data(GYRO_XOUT_H)
gyro_y = read_raw_data(GYRO_YOUT_H)
gyro_z = read_raw_data(GYRO_ZOUT_H)
#Full scale range +/- 250 degree/C as per sensitivity scale factor
Ax = acc_x/16384
Ay = acc_y/16384
Az = acc_z/16384
Gx = gyro_x/131.0
Gy = gyro_y/131.0
Gz = gyro_z/131.0
gyro_x_angle= Gy * DT
accel_x_angle = (math.atan2(Az,Ax)-(math.pi/2))*rad_to_deg
time.sleep(DT)
似乎有很多类似的自平衡机器人项目,但我能找到的用 python 编程的很少 任何帮助将不胜感激。
我尝试过列表、for循环、while循环,但似乎无法获得所需的结果。