树莓派：MPU6050控制舵机转向

舵机转向"/>

树莓派：MPU6050控制舵机转向

一、材料清单

1. 树莓派3B+
2. MPU6050
3. PCA9685扩展板(输出16路pwm)
4. 一个舵机
5. 面包板、GPIO扩展板(可有可无，只是方便接在面包板上）

二、电路接线

1. PCA9685和树莓派的接线 
   1. SCL---->SCL1(树莓派)
   2. SDA---->SDA1(树莓派)
   3. VCC---->+5V(给PCA9685芯片供电)
   4. GND---->GND(树莓派)
   5. V+ ------>外接一个电源(树莓派IO供电不足)
2. 舵机接PCA9685(略)
3. MPU6050接树莓派
   1. SCL-------->SCL1(树莓派)
   2. SDA-------->SDA1(树莓派)

三、程序说明

1. 建立两个线程
   1. MPU6050------> 通过四元数算法转换成欧拉角(偏航角，翻滚角，俯仰角)
   2. PCA9685------> 驱动舵机转向
2. 数据读取
   1. 建一个线程队列QUEUE
      1. 在MPU6050线程里，判断队列为空，则将MPU6050读取到的角度存入队列里
      2. 在PCA9685线程里，判断队列不为空，则PCA9685驱动函数读取出角度，再转换成舵机对应的角度(此例程舵机初始角度为90°，即当读取的角度为0时，舵机维持在90度，为了可以使舵机左右摆动）
3. 关于四元数转换成欧拉角：

四、主要程序

【注】部分模块可以在Pypi网站上下载

import threading
import Adafruit_PCA9685
from mpu6050 import mpu6050
import time,math,queue
import MPU6050filtermy_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 -1def 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....我怀疑是杜邦线的问题，出现了很多次这种情况。