陀螺仪平衡小车实验

    之前曾尝试如何获取陀螺仪的数据,文章地址: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、由于没有经过滤波,所以小车很容易出现抖动。
视频效果:
另一个视角:

L298N驱动板驱动实验

    以前曾用L298N改进板驱动直流电机,文章链接:https://www.shumeijiang.com/2020/05/04/直流电机驱动变速实验.html;现在尝试L298N驱动板如何接线以及如何使用。
驱动板注解
    其中改进型和这个板子的明显区别在于,改进型将调速直接作用于高电平引脚输出即可;这个板子,在ENA和ENB上面作用;如果安装跳帽则表示固定速度,如果连接树莓派引脚,则可以通过PWM调速。
#接线示例
#代码示例

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

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

import RPi.GPIO as GPIO ##引入GPIO模块
import time    ##引入time库

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)

#enA决定速度
pwm = GPIO.PWM(enA, 80)
pwm.start(70)  ##初始化占空比 即单位时间高电平的时间占比

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

##通过一定时间间隔设置占空比,可见电机间隔一定时间后会发生速度变化 其中0则电机停止转动
try:
    while True:
        pwm.ChangeDutyCycle(30)
        time.sleep(2)
        pwm.ChangeDutyCycle(70)
        time.sleep(2)
        pwm.ChangeDutyCycle(0)  ##电机停止转动
        time.sleep(2)  ##持续时间

except KeyboardInterrupt:
    pass
pwm.stop()
GPIO.cleanup()
#视频效果:

四驱小车循迹实验-循迹实现

    前两篇文章已经介绍了如何组装四驱小车以及小车的驱动类封装;接下来这篇文章将尝试驱动类调用以及小车循迹测试。
    小车组装文章:https://www.shumeijiang.com/2021/09/23/四驱小车循迹实验-小车组装/。
    小车驱动类封装文章:https://www.shumeijiang.com/2021/09/23/四驱小车循迹实验-直流电机驱动封装/
#循迹实验效果
#实现效果:
    从上面gif图以及视频可看到,小车遇到直线会直行前进,由于手工贴的胶带不是很直所以小车会实时调整前进的角度;当遇到转弯时,同样会一点点进行转弯直到寻找到黑线线条;
传感器示例
    上图模拟小车前面并排的三个红外避障传感器,分别是ONE,TWO,THREEE;默认黑线位于TWO传感器正下方为直行;由于小车要不停向前移动,但是黑线有弯曲或者转弯的情况,所以小车会出现三个传感器分别接触黑线的情况;通过不同的传感器被触发事件,从而得出小车现在的行走方向以及判断是否要做方向调整,具体原理如下(1为检测到黑线触发事件,0为未检测到黑线):

1、ONE=1,THREE=0 说明黑线偏转右侧,此时小车应向右转向;
2、ONE=0,THREE=1 说明黑线偏转左侧,此时小车应向左转向;
3、ONE=0,TWO=1,THREE=0 说明黑线在中心方向,小车需直行;
4、ONE=1,TWO=1,THREE=1 说明遇到横线,小车需停顿;
5、ONE=0,TWO=0,THREE=0 说明小车失去黑线,此时需寻找或者停止。
#实现代码

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

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

import RPi.GPIO as GPIO
import time

GPIO.setwarnings(False)

#引入驱动类
from baseAct import baseAct
#实例化驱动类并赋值四个电机所占引脚值
act = baseAct(17, 16, 13, 12, 19, 18, 21, 20)

#三个传感器检测黑线
senseOne = 22
senseTwo = 23
senseThree = 24

#一个检测障碍物
checkPin = 25

GPIO.setmode(GPIO.BCM)
GPIO.setup(senseOne, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(senseTwo, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(senseThree, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(checkPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)

#事件注册 注意不同传感器触发事件不同
GPIO.add_event_detect(senseOne, GPIO.RISING, bouncetime=200)
GPIO.add_event_detect(senseTwo, GPIO.FALLING, bouncetime=200)
GPIO.add_event_detect(senseThree, GPIO.RISING, bouncetime=200)
GPIO.add_event_detect(checkPin, GPIO.FALLING, bouncetime=200)

#检测事件
def check_event(pin):
    status = GPIO.event_detected(pin)
    return status

#检测状态
def check_status(pin):
    status = GPIO.input(pin)
    return status

#find
done = False
try:
    while not done:
   
        #障碍物检测
        front_status = check_event(checkPin)
        print("front status is %d"% front_status)
        if front_status == 1:
            print('有障碍物 停止')
            act.act_backward(0.5, 50, 50)
            done = True
            break

        #检测各传感器状态
        #status = find_way(senseOne, senseTwo, senseThree)
        sOne = check_event(senseOne)
        sTwo = check_event(senseTwo)
        sThree = check_event(senseThree)

        print('sOne is %d'% sOne)
        print('sTwo is %d'% sTwo)
        print('sThreee is %d' % sThree)
       
        way_type = 'forward'
        #继续前行
        if sOne == 0 and sThree==0:
            act.act_forward(0.15, 50, 40, True)
            print('forward')

        #遇到横线停顿
        elif sOne == 1 and sThree == 1:
            time.sleep(0.5)
            print('wait')

        #向右侧转弯调整
        elif sOne == 1 and sThree == 0:
            #act.turn_right(0.15, 50, 40)

            #检测是否居中
            find = False
            while not find:
                oneStatus = check_event(senseOne)
                twoStatus = check_event(senseTwo)
                threeStatus = check_event(senseThree)
                print('run right---->>')
                print('one status is %d'% oneStatus)
                print('two status is %d'% twoStatus)
                print('three status is %d'% threeStatus)

                if oneStatus==0 and twoStatus==1 and threeStatus==0:
                    find = True
                    break

                act.turn_right(0.15, 55, 45, True)
                time.sleep(0.2)
                   
            print('right')
            way_type = 'right'

        #向左侧转弯调整
        elif sOne == 0 and sThree == 1:

            find = False
            while not find:
                oneStatus = check_event(senseOne)
                twoStatus = check_event(senseTwo)
                threeStatus = check_event(senseThree)
                print('run left<<----')
                print('one status is %d'% oneStatus)
                print('two status is %d'% twoStatus)
                print('three status is %d'% threeStatus)

                if oneStatus==0 and twoStatus==1 and threeStatus==0:
                    find = True
                    break

                act.turn_left(0.15, 55, 45, True)
                time.sleep(0.2)

            print('left')
            way_type = 'left'
        else:
            pass

        print('once')
        #time.sleep(0.2)

        #if sOne==0 and sTwo==0 and sThree==0:
            #act.act_break()
            #print('stop!!!!')
            #break

except KeyboardInterrupt:
    pass

GPIO.cleanup()
从上面的代码可以看到循迹的策略和流程如下:

1、首先注册one和three分别一个RISING(低变高)事件,因为默认情况下这两个传感器没有遇到黑线而是地板,所以触发低电平事件(传感器低电平触发检测);

2、two注册一个FALLING(高变低)事件,因为默认two传感器红外被黑色线条吸收,从而不产生障碍物检测,所以检测状态为高电平;

3、checkPin为小车前方障碍物检测,如果遇到前方障碍物,小车停车并后退,这个不是重点不做详细介绍;

4、one、two、three传感器的防抖时间bouncetime,异或说检测间隔为200毫秒,这个可以自己调节,主要用于探测黑线的频率;

5、紧接着进入一个while循环,每次都检测三个传感器状态,当发现:
(1)one和three都没有触发RISING事件,我们认为此时黑线还在中间,所以直行;
(2)one和three都触发RISING事件,说明遇到横线,所以停顿;
(3)one触发,three没有触发,说明直行后黑线弯曲,触发了右侧one传感器;为了黑线居中,这个时候小车就需要右转,但是由于不知道黑线弯曲程度(后续可实验图像识别),所以小车进入一个找线环节,以0.15秒的步伐,一点点右转,同时占空比调为55,赫兹设置为45,降低右转速度;当two传感器触发FALLING事件,说明小车归正,则继续触发直行策略,以此类推;
(4)one没有触发,three触发,说明直行后黑线向左弯曲,触发左转动作,具体细节同右转相似;

6、如此,小车便会直行、停顿、左转和右转;当然程序还有很多不足的地方,后续还会继续优化。
#事件检测(边沿检测)部分可参考文章:https://www.shumeijiang.com/2020/03/15/gpio之event_detected系列函数/

四驱小车循迹实验-直流电机驱动封装

    上篇文章已经介绍了四驱小车如何组装,这篇文章我们将继续介绍如何驱动小车前进后退以及左转右转;因为小车循迹的过程中,会遇到左转线、直线、右转线以及停顿标示;这个时候就需要小车能实现左转、右转、直行、停车、后退等动作。
#直流电机驱动板L298N驱动原理:
直流电机转动效果IN1IN2IN3IN4
MOTOR-A正向(调速)高电平/PWM低电平
反向(调速)低电平高电平/PWM
停止低电平低电平
刹车高电平高电平
MOTOR-B正向(调速)高电平/PWM低电平
反向(调速)低电平高电平/PWM
停止低电平低电平
刹车高电平高电平
    从上面表格可见,motorA连接引脚IN1和IN2,当IN1高电平IN2低电平的时候,电机正向转动,其中IN1输入PWM进行调速和扭矩变化;反之IN1低电平和IN2高电平的时候电机反向转动,从而实现倒车;其他电机以此类推。
#其中PWM控制转速和扭矩,示例如下:
1、创建PWM实例:p = GPIO.PWM(channel, frequency);
2、开始PWM:p.start(duty_cycle)  #duty_cycle为占空比;
3、更改频率,更改扭矩:p.ChangeFrequency(frequency);
4、更改占空比,更改转速:p.ChangeDutyCycle(duty_cycle);
5、PWM停止:p.stop()
#代码示例

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

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

import RPi.GPIO as GPIO ##引入GPIO模块
import time    ##引入time库
GPIO.setmode(GPIO.BCM)

#驱动类
class baseAct:
    def __init__(self, ain1, ain2, ain3, ain4, bin1, bin2, bin3, bin4):
        self.ain1 = ain1
        self.ain2 = ain2
        self.ain3 = ain3
        self.ain4 = ain4
        self.bin1 = bin1
        self.bin2 = bin2
        self.bin3 = bin3
        self.bin4 = bin4
        GPIO.setup(self.ain1, GPIO.OUT)
        GPIO.setup(self.ain2, GPIO.OUT)
        GPIO.setup(self.ain3, GPIO.OUT)
        GPIO.setup(self.ain4, GPIO.OUT)
        GPIO.setup(self.bin1, GPIO.OUT)
        GPIO.setup(self.bin2, GPIO.OUT)
        GPIO.setup(self.bin3, GPIO.OUT)
        GPIO.setup(self.bin4, GPIO.OUT)

    #结束动作
    def act_stop(self, duration, slow_time=0.15):

        #执行时间
        duration = float(duration)-slow_time
        time.sleep(duration)
   
        #self.init_move('forward', duration, 50, 50)

        if slow_time:
            #执行缓冲慢行
            self.apwm1.ChangeDutyCycle(20)
            self.apwm3.ChangeDutyCycle(20)
            self.bpwm1.ChangeDutyCycle(20)
            self.bpwm3.ChangeDutyCycle(20)
            time.sleep(slow_time)

        #结束动作
        self.apwm1.stop()
        self.apwm3.stop()
        self.bpwm1.stop()
        self.bpwm3.stop()
        GPIO.output(self.ain2, GPIO.LOW)
        GPIO.output(self.ain4, GPIO.LOW)
        GPIO.output(self.bin2, GPIO.LOW)
        GPIO.output(self.bin4, GPIO.LOW)

    ##PWM初始化
    def init_move(self, act_type, duration, duty_ratio, hz_num):

        if act_type == 'forward':
            hpin1 = self.ain1
            lpin1 = self.ain2
            hpin2 = self.ain3
            lpin2 = self.ain4
            hpin3 = self.bin1
            lpin3 = self.bin2
            hpin4 = self.bin3
            lpin4 = self.bin4
        elif act_type == 'backward':
            hpin1 = self.ain2
            lpin1 = self.ain1
            hpin2 = self.ain4
            lpin2 = self.ain3
            hpin3 = self.bin2
            lpin3 = self.bin1
            hpin4 = self.bin4
            lpin4 = self.bin3
        elif act_type == 'turn_left':
            hpin1 = self.ain1  
            lpin1 = self.ain2  #右前电机前进 pin2低电平则前进
            hpin2 = self.ain3
            lpin2 = self.ain4  #右后电机前进
            hpin3 = self.bin2
            lpin3 = self.bin1  #左前电机后退 pin2高电平则后退
            hpin4 = self.bin4
            lpin4 = self.bin3  #左后电机后退
        elif act_type == 'turn_right':
            hpin1 = self.ain2
            lpin1 = self.ain1  #右前电机后退  pin2高电平则后退
            hpin2 = self.ain4
            lpin2 = self.ain3  #右后电机后退
            hpin3 = self.bin1
            lpin3 = self.bin2  #左前电机前进  pin2高电平则前进
            hpin4 = self.bin3
            lpin4 = self.bin4  #左后电机前进
        else:
            print "不支持的类型"
            pass

        #挨个实例化每个电机的PWM实例
        self.apwm1 = GPIO.PWM(hpin1, hz_num) #实例PWM 并可自定义hz
        self.apwm1.start(0)           #初始占空比为0
        GPIO.output(hpin1, GPIO.HIGH) #设置单个电机一个高电压
        GPIO.output(lpin1, GPIO.LOW)  #另一个低电压
        self.apwm1.ChangeDutyCycle(duty_ratio) #设置用户自定义占空比

        #驱动第二个电机
        self.apwm3 = GPIO.PWM(hpin2, hz_num)
        self.apwm3.start(0)
        GPIO.output(hpin2, GPIO.HIGH)
        GPIO.output(lpin2, GPIO.LOW)
        self.apwm3.ChangeDutyCycle(duty_ratio)

        #驱动第三个电机
        self.bpwm1 = GPIO.PWM(hpin3, hz_num)
        self.bpwm1.start(0)
        GPIO.output(hpin3, GPIO.HIGH)
        GPIO.output(lpin3, GPIO.LOW)
        self.bpwm1.ChangeDutyCycle(duty_ratio)

        #驱动第四个电机
        self.bpwm3 = GPIO.PWM(hpin4, hz_num)
        self.bpwm3.start(0)
        GPIO.output(hpin4, GPIO.HIGH)
        GPIO.output(lpin4, GPIO.LOW)
        self.bpwm3.ChangeDutyCycle(duty_ratio)

    ##前进
    def act_forward(self, duration, duty_ratio, hz_num=50, close_slow=False):
        self.init_move('forward', duration, duty_ratio, hz_num)

        if close_slow:
            slow_time = 0
        else:
            slow_time = 0.15

        #停止动作 并执行缓冲
        self.act_stop(duration, slow_time)

    ##后退
    def act_backward(self, duration, duty_ratio, hz_num):
        self.init_move('backward', duration, duty_ratio, hz_num)
        #停止动作 并缓冲执行
        self.act_stop(duration)

    ##向左转
    def turn_left(self, duration, duty_ratio, hz_num, serial=False):
        self.init_move('turn_left', duration, duty_ratio, hz_num) #左转执行
        slow_time = 0.05  #减速时间
        if serial==True:
            slow_time = 0
        self.act_stop(duration, slow_time)  #动作停止

    ##向右转
    def turn_right(self, duration, duty_ratio, hz_num, serial=False):
        self.init_move('turn_right', duration, duty_ratio, hz_num) #右转执行
        slow_time = 0.05  #减速时间
        if serial==True:
            slow_time = 0 #连续不减速
        self.act_stop(duration, slow_time)  #动作停止

    ##刹车
    def act_break(self):
        GPIO.output(self.ain1, GPIO.HIGH)
        GPIO.output(self.ain2, GPIO.HIGH)
        GPIO.output(self.ain3, GPIO.HIGH)
        GPIO.output(self.ain4, GPIO.HIGH)

        GPIO.output(self.bin1, GPIO.HIGH)
        GPIO.output(self.bin2, GPIO.HIGH)
        GPIO.output(self.bin3, GPIO.HIGH)
        GPIO.output(self.bin4, GPIO.HIGH)
    如上代码可见,封装了直流电机驱动类baseAct,以及直行、后退、左转、右转和刹车等方法;实例化类后便可直接调用,其中ain1、ain2、ain3、ain4为左侧电机驱动板引脚,bin1、bin2、bin3、bin4为右侧电机驱动板引脚,分别对应即可。
#参考文章:https://www.shumeijiang.com/2020/05/04/直流电机驱动变速实验/

直流电机驱动变速实验

#实验目的:通过测试外接可变压电源以及直流电机驱动板(L298N)这个组合,驱动自制的风扇,在pwm的调节下,展现转动速度连续发生变化的现象。
#接线效果如图:
接线示例
#注:其中风扇造型可采用其他形式代替,只要可体现转动速度即可。
#驱动板:采用的是L298N的改进型,某宝有卖;他跟传统L298N驱动板控制pwm不同在于,它直接在对应电机的高电平引脚输出即可;单个板可同时驱动两个电机。具体接线效果如下:
#控制原理如下: 
直流电机转动形式IN1IN2IN3IN4
MOTOR-A正向(调速)高电平/PWM低电平
反向(调速)低电平高电平/PWM
停止低电平低电平
刹车高电平高电平
MOTOR-B正向(调速)高电平/PWM低电平
反向(调速)低电平高电平/PWM
停止低电平低电平
刹车高电平高电平
#实验代码:
代码示例

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

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

import RPi.GPIO as GPIO ##引入GPIO模块
import time    ##引入time库

motorA1 = 18   ##定义电机A的IN1引脚
motorA2 = 19   ##定义电机A的IN2引脚

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

pwm = GPIO.PWM(motorA1, 80)    ##引脚A1高电平为正转,所以创建一个正转pwm实例并设置频率
pwm.start(70)  ##初始化占空比 即单位时间高电平的时间占比

GPIO.output(motorA1, GPIO.HIGH) ##设置A1 引脚为高电平
GPIO.output(motorA2, GPIO.LOW)  ##设置A2 引脚为低电平 如此可控制电机A正转,反之电机A反转

##通过一定时间间隔设置占空比,可见电机间隔一定时间后会发生速度变化 其中0则电机停止转动
try:
    while True:
        pwm.ChangeDutyCycle(30)
        time.sleep(2)
        pwm.ChangeDutyCycle(70)
        time.sleep(2)
        pwm.ChangeDutyCycle(0)  ##电机停止转动
        time.sleep(2)  ##持续时间

except KeyboardInterrupt:
    pass

pwm.stop()
GPIO.cleanup()
#实验效果:
1、执行代码 Python jiujiang.py;
2、可见小风扇快速转动,然后持续2秒钟;速度增强到原来两倍;
3、速度增强转动2秒钟后,风扇停止转动;
4、停转2秒后,风扇继续往复前面一个动作。
#视频效果如下: