树莓派：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. 关于四元数转换成欧拉角：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....我怀疑是杜邦线的问题，出现了很多次这种情况。