一、材料清单
- 树莓派3B+
- MPU6050
- PCA9685扩展板(输出16路pwm)
- 一个舵机
- 面包板、GPIO扩展板(可有可无,只是方便接在面包板上)
二、电路接线
- PCA9685和树莓派的接线
- SCL---->SCL1(树莓派)
- SDA---->SDA1(树莓派)
- VCC---->+5V(给PCA9685芯片供电)
- GND---->GND(树莓派)
- V+ ------>外接一个电源(树莓派IO供电不足)
- 舵机接PCA9685(略)
- MPU6050接树莓派
- SCL-------->SCL1(树莓派)
- SDA-------->SDA1(树莓派)
三、程序说明
- 建立两个线程
- MPU6050------> 通过四元数算法转换成欧拉角(偏航角,翻滚角,俯仰角)
- PCA9685------> 驱动舵机转向
- 数据读取
- 建一个线程队列QUEUE
- 在MPU6050线程里,判断队列为空,则将MPU6050读取到的角度存入队列里
- 在PCA9685线程里,判断队列不为空,则PCA9685驱动函数读取出角度,再转换成舵机对应的角度(此例程舵机初始角度为90°,即当读取的角度为0时,舵机维持在90度,为了可以使舵机左右摆动)
- 建一个线程队列QUEUE
- 关于四元数转换成欧拉角:https://blog.csdn.net/weixin_38956024/article/details/94757023
四、主要程序
【注】部分模块可以在Pypi网站上下载
import threading
import Adafruit_PCA9685
from mpu6050 import mpu6050
import time,math,queue
import MPU6050filter
my_queue = queue.Queue()
def t_mpu6050():
sensor = mpu6050(address=0x68) # 设备地址
sensor.set_accel_range(mpu6050.ACCEL_RANGE_16G) # 设置加速度计的量程
sensor.set_gyro_range(mpu6050.GYRO_RANGE_2000DEG) # 设置陀螺仪的量程
while 1:
accel_data = sensor.get_accel_data() # 读取加速度计x,y,z ,返回是字典
gyro_data = sensor.get_gyro_data() # 读取陀螺仪的x,y,z, 返回是字典
# 四元素转欧拉角
rotation = MPU6050filter.IMUupdate(accel_data['x'],accel_data['y'],accel_data['z'],
gyro_data['x'],gyro_data['y'],gyro_data['z'])
# 如果队列为空,则存入一个角度(我想用偏航角来控制舵机),保证只存一个角度在队列里
if my_queue.empty():
my_queue.put(rotation['Yaw'])
print(rotation['Yaw'])
time.sleep(0.1) # 设置一个延时,可以让它不那么灵敏
def t_servo():
pwm = Adafruit_PCA9685.PCA9685() # 创建一个实例
pwm.set_pwm_freq(50) # 设置pwm的周期频率
# 将输入的角度,转换成PCA9685的数值(12位精度)
def set_servo_angle(channel, angle):
date = 4096 * ((angle * 11) + 500) / 20000
pwm.set_pwm(channel, 0, int(date))
#循环接收队列里的角度
while 1:
if not my_queue.empty():
get_x = my_queue.get()
get_x = 90 + get_x # 90度为基础角,为了可以左右摆动
set_servo_angle(0, get_x) # 设置通道0的舵机转向
def main():
# 创建线程
Tmpu6050 = threading.Thread(target=t_mpu6050)
Tservo = threading.Thread(target=t_servo)
# 开始线程
Tmpu6050.start()
Tservo.start()
Tmpu6050.join()
Tservo.join()
if __name__ == "__main__":
main()
五、程序补充
如果只想简单地获取 翻滚角和俯仰角
可以通过解析加速度的数据
可以使用如下程序:
def dist(a, b):
return math.sqrt((a*a) + (b*b))
def get_x_rotation(x,y,z):
try:
radians = math.atan(x / dist(y,z))
return math.degrees(radians)
except ZeroDivisionError:
print("error!")
return -1
def get_y_rotation(x,y,z):
try:
radians = math.atan(y/dist(x,y))
return math.degrees(radians)
except ZeroDivisionError:
print("error!")
return -1
# 如获取翻滚角
accel_data = sensor.get_accel_data()
x = get_x_rotation(accel_data['x'],accel_data['y'],accel_data['z'])
六、测试效果
1、读取的角度
2、舵机接入5V, 最大电流的峰值为1.4A
3、频繁移动MPU6050时,突然读取的数据全为0....我怀疑是杜邦线的问题,出现了很多次这种情况。