陀螺仪平衡小车实验

陀螺仪平衡小车实验

    之前曾尝试如何获取陀螺仪的数据,文章地址:https://www.shumeijiang.com/2021/11/09/陀螺仪模块实验-获取欧拉角.html;今天则尝试给陀螺仪加上两个轮子组装成一个二轮小车;然后利用陀螺仪数据使之保持平衡。组装小车中使用到树莓派Zero,直流电机驱动板L298N,以及可显降压升高模块。
接线示例:
实现原理:
1、获取陀螺仪Roll数据(根据陀螺仪安装方向而定);
2、定义一个平衡区间,然后对比陀螺仪的值是否在区间内;
3、当数值小于左侧边界时,小车向左移动来保持平衡;
4、当数值大于右侧边界时,小车向右移动来保持平衡;
5、当在区间内时,小车停止驱动。
代码示例:

#!/usr/bin/env python
#coding:utf-8

'''
from JiuJiang
树莓酱的操作实例
https:://www.shumeijiang.com
'''

from mpu6050 import mpu6050
#import threading
import time
import MPU6050filter
import RPi.GPIO as GPIO ##引入GPIO模块

motorA1 = 17   ##定义电机A的IN1引脚
motorA2 = 18   ##定义电机A的IN2引脚
enA = 21 #引脚21接ENA,调速电机A

GPIO.setmode(GPIO.BCM)  ##此处采用BCM编码
GPIO.setup(motorA1, GPIO.OUT)  ##设置引脚为输出模式
GPIO.setup(motorA2, GPIO.OUT)
GPIO.setup(enA, GPIO.OUT)

#初始化PWM
pwm = GPIO.PWM(enA, 40)
pwm.start(60)  ##初始化占空比

#陀螺仪数据获取
def get_mpu6050():
    sensor = mpu6050(address=0x68)
    sensor.set_gyro_range(mpu6050.ACCEL_RANGE_16G)
    sensor.set_accel_range(mpu6050.GYRO_RANGE_2000DEG)
    time.sleep(0.02)

    accel_data = sensor.get_accel_data()
    gyro_data = sensor.get_gyro_data()
    rotation = MPU6050filter.IMUupdate(accel_data['x'], accel_data['y'], accel_data['z'], gyro_data['x'], gyro_data['y'], gyro_data['z'])

    return rotation

#设置正反方向
def get_direction(direction):
        if direction == 1:
            GPIO.output(motorA1, GPIO.LOW)
            GPIO.output(motorA2, GPIO.HIGH)
        else:
            GPIO.output(motorA1, GPIO.HIGH) ##设置A1 引脚为高电平
            GPIO.output(motorA2, GPIO.LOW)  ##设置A2 引脚为低电平 如此可控制电机A正转,反之电机A反转

#根据幅度决定速度
def get_duty_cycle(val, n=1):
    val = abs(val)
    if val>10:
        val = 10
    return n*(val*6+40)

def main():
    middle = -3
    left_board = -9
    right_board = 6
    while True:
        time.sleep(0.05)
        rotation = get_mpu6050()
        roll = rotation[1]
        val = roll-middle
        print("roll is", roll)

        duty = get_duty_cycle(val, 0.6)
        if val > right_board:
            get_direction(2)
            pwm.ChangeDutyCycle(duty)
        elif val < left_board:
            get_direction(1)
            pwm.ChangeDutyCycle(duty)
        else:
            #安全区域
            pwm.ChangeDutyCycle(0)

if __name__ == '__main__':
    main()
平衡效果:
实验效果:
1、执行命令 Python jiujiang.py;
2、当小车中心失衡时,出现向左向右行走以保持平衡;
3、不过也有很多问题,当偏恒幅度过大时,小车容易出现无法保持平衡的情况;
4、由于没有经过滤波,所以小车很容易出现抖动。
视频效果:
另一个视角:
jiujiang

评论已关闭。