树莓派sudo 树莓派算法

它身上有前中后三个超声波传感器，轻松调用他们就能使小车避障轻松走出迷宫啦。预期小车走的路线是这样子的：

我的核心思路：

初始小车为前进状态，此时使用前、左、右三个传感器检测小车三边与障碍物的距离，当前方距离小于一个界限值时，判断左右传感器与障碍物的距离，如果左大于右，则向左转，否则，向右。当转向完成后小车的转向角度可能会发生偏差，判断左右传感器与障碍物的距离，设置一个大致的调整时间，使小车停止，然后进行微调整，再返回循环体继续前进。

使小车自己进行微微调整的思路：
转向完成后，判断左右传感器与障碍物的距离，如果小于一个界限值，则令小车进行微调。使小车有一个较好的角度前进，将碰撞的可能性降到最低。

程序流程图：

代码如下：

# -*- coding: utf-8 -*-

import RPi.GPIO as gpio  # 第一行引用后，可以设置中文注释  树莓派上才有的
import time
import sys
import random

gpio.setwarnings(False)  # 去掉一些不必要的警告


def init():
    gpio.setmode(gpio.BOARD)  # GPIO调用BOARD编号方式
    gpio.setup(38, gpio.OUT)
    gpio.setup(40, gpio.IN)
    gpio.setup(35, gpio.OUT)
    gpio.setup(37, gpio.IN)
    gpio.setup(29, gpio.OUT)
    gpio.setup(31, gpio.IN)


def gp():
    gpio.setmode(gpio.BOARD)
    gpio.setup(12, gpio.OUT)  # 引脚设置为输出
    gpio.setup(16, gpio.OUT)
    gpio.setup(18, gpio.OUT)
    gpio.setup(22, gpio.OUT)


# 前传感器
def forward_distance():
    init()
    gpio.output(38, True)  # 发出触发信号保持10us以上（15us）
    time.sleep(0.000015)
    gpio.output(38, False)
    while not gpio.input(40):
        pass
    t1 = time.time()  # 发现高电平时开时计时
    while gpio.input(40):
        pass
    t2 = time.time()  # 高电平结束停止计时
    return (t2 - t1) * 34000 / 2  # 返回距离，单位为厘米
    gpio.cleanup()
    return forward_distance


# 左传感器
def left_distance():
    init()
    gpio.setup(35, gpio.OUT)
    gpio.output(35, True)  # 发出触发信号保持10us以上（15us）
    time.sleep(0.000015)
    gpio.output(35, False)
    while not gpio.input(37):
        pass
    t1 = time.time()  # 发现高电平时开时计时
    while gpio.input(37):
        pass
    t2 = time.time()  # 高电平结束停止计时
    return (t2 - t1) * 34000 / 2  # 返回距离，单位为厘米
    gpio.cleanup()
    return left_distance


# 右传感器
def right_distance():
    init()
    gpio.output(29, True)  # 发出触发信号保持10us以上（15us）
    time.sleep(0.000015)
    gpio.output(29, False)
    while not gpio.input(31):
        pass
    t1 = time.time()  # 发现高电平时开时计时
    while gpio.input(31):
        pass
    t2 = time.time()  # 高电平结束停止计时
    return (t2 - t1) * 34000 / 2  # 返回距离，单位为厘米
    gpio.cleanup()
    return right_distance


def forward():
    init()
    gp()
    # 全速前进状态
    gpio.output(12, True)  # 左前进
    gpio.output(16, False)  # 左后退
    gpio.output(22, False)  # 右后退
    gpio.output(18, True)  # 右前进
    # q = gpio.PWM(12, 50)  # !error 两个PWM信号程序会出错
    # p = gpio.PWM(18, 50)  # 信道，频率
    # q.start(50)
    # p.start(50)  # 占空比
    time.sleep(0)


# 左转
def left(runtime):
    gp()
    gpio.output(12, False)
    gpio.output(16, False)
    gpio.output(22, False)
    gpio.output(18, True)
    p = gpio.PWM(18, 80)  # 信道，频率
    p.start(50)  # 占空比
    time.sleep(runtime)
    gpio.cleanup()


# 右转
def right(runtime):
    gp()
    gpio.output(12, True)
    gpio.output(16, False)
    gpio.output(22, False)
    gpio.output(18, False)
    p = gpio.PWM(12, 80)  # 信道，频率
    p.start(50)  # 占空比
    time.sleep(runtime)
    gpio.cleanup()


# 左边微调
def adjust_left(runtime):
    init()
    gp()
    gpio.output(12, False)  # 左前进
    gpio.output(16, True)  # 左后退
    gpio.output(22, False)  # 右后退
    gpio.output(18, True)  # 右前进
    # q = gpio.PWM(16,50)
    # p = gpio.PWM(18,50)
    # q.start(50)
    # p.start(50)
    time.sleep(runtime)


# 右边微调
def adjust_right(runtime):
    init()
    gp()
    gpio.output(12, True)  # 左前进
    gpio.output(16, False)  # 左后退
    gpio.output(22, True)  # 右后退
    gpio.output(18, False)  # 右前进
    # q = gpio.PWM(12, 50)
    # p = gpio.PWM(22, 50)
    # q.start(50)
    # p.start(50)
    time.sleep(runtime)


def stop(runtime):
    init()
    gp()
    gpio.output(12, False)
    gpio.output(16, False)
    gpio.output(22, False)
    gpio.output(18, False)
    gpio.cleanup()
    time.sleep(runtime)


def run():
    while True:
        init()
        gp()
        forward()
        fd = forward_distance()
        ld = left_distance()
        rd = right_distance()
        print('前距离是：%0.2f cm' % fd)
        print("----------------------")
        if ld < 7:
            stop(0.3)
            adjust_left(0.5)
            print("向左微转")
            continue
        if rd < 7:
            stop(0.3)
            adjust_right(0.5)
            print("向右微转")
            continue
        if fd > 200:
            stop(0.3)
            adjust_left(0.5)
            continue
        if fd < 30 and ld > rd:
            print("左转")
            print("----------------------")
            print('左距离是：%0.2f cm' % ld)
            left(2)
            continue
        elif fd < 30 and rd > ld:
            print("右转")
            print("----------------------")
            print('右距离是：%0.2f cm' % rd)
            right(1.25)
            continue
        else:
            continue


run()

总体是非常简单的，代码也有注释，希望对你们有帮助！