树莓派: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....我怀疑是杜邦线的问题,出现了很多次这种情况。

MPU6050是一种常用的六轴传感器模块,可以同时测量加速度和角速度。它通常与Arduino等微控制器一起使用,用于姿态测量、运动控制等应用。控制舵机是其中一种常见的应用场景之一。 要使用MPU6050控制舵机,首先需要连接MPU6050和Arduino。MPU6050通过I2C总线与Arduino通信,需要将其SDA引脚连接到Arduino的SDA引脚,SCL引脚连接到Arduino的SCL引脚。接下来,你需要编写Arduino代码来读取MPU6050的数据,并根据需要控制舵机的角度。 以下是一个简单的示例代码,演示了如何使用MPU6050控制舵机: ```cpp #include <Wire.h> #include <Servo.h> #include <MPU6050.h> MPU6050 mpu; Servo servo; void setup() { Wire.begin(); mpu.initialize(); servo.attach(9); // 将舵机连接到Arduino的9号引脚 } void loop() { int16_t gyroX = mpu.getRotationX(); // 获取X轴角速度 int angle = map(gyroX, -32768, 32767, 0, 180); // 将角速度映射到舵机角度范围内 servo.write(angle); // 控制舵机角度 delay(20); // 延时一段时间 } ``` 在这个示例中,我们使用了Wire库来进行I2C通信,Servo库来控制舵机MPU6050库来读取MPU6050的数据。在setup函数中,我们初始化了MPU6050和舵机。在loop函数中,我们通过mpu.getRotationX()函数获取X轴的角速度,并将其映射到舵机的角度范围内,然后使用servo.write()函数控制舵机的角度。最后,通过delay函数延时一段时间,以便观察舵机的运动。 希望以上信息对你有帮助!如果你还有其他问题,请继续提问。
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值