舵机转向"/>
树莓派:MPU6050控制舵机转向
一、材料清单
- 树莓派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
- 关于四元数转换成欧拉角:
四、主要程序
【注】部分模块可以在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....我怀疑是杜邦线的问题,出现了很多次这种情况。
更多推荐
树莓派:MPU6050控制舵机转向
发布评论