Micropython TPYBoard V10X系列之超声波避障小车

超声波避障小车

一、实验器材

1、TPYboard V102板  1块

2、电机驱动模块L298N   1个

3、电机 2块

4、小车底盘 1个

5、超声波模块 1个

6、舵机 1个

7、SG90舵机支架 1个

8.超声波云台支架 1个

二、超声波模块

1、什么是超声波模块

   超声波传感器是利用超声波的特性研制而成的传感器, 它是通过传送一个超声波（远高于人的听觉范

围）和提供一个对应于爆裂回声返回到传感器所需时间的输出脉冲来工作的。超声波传感器在非接触性测量

方面的应用非常广泛，如检测液体水位（特别是具有腐蚀性的液体，如硫酸、硝酸液体），汽车倒车防撞系

统，金属/非金属探伤等，都可以用到超声波距离传感器。

2、超声波模块测距的原理

(1)采用IO口TRIG触发测距，给最少10us的高电平信呈。

(2)模块自动发送 8 个 40khz 的方波，自动检测是否有信号返回。

(3)有信号返回，通过 IO 口 ECHO 输出一个高电平，高电平持续的时间就是超声波从发射到返回的时间。测

试距离=(高电平时间*声速(340M/S))/2。

如下图接线，VCC 供 5V电源， GND 为地线，TRIG 触发控制信号输入，接板子的X9，ECHO 回响信号输出，

接板子的X10，四个接口端。

三、电机驱动模块

1、什么是电机驱动模块

电机驱动模块主要是可以控制电机的运行：调速、运行、停止、步进、匀速等操作

2、L298N的使用方法

   L298N模块是2路的H桥驱动，所以可以同时驱动两个电机，接法如图所示使能ENA ENB之后，可以分别从

IN1 IN2输入PWM信号驱动电机1的转速和方向，可以分别从IN3 IN4输入PWM信号驱动电机2的转速和方向。我

们将电机1接口的OUT1与OUT2与小车的一个电机的正负极连接起来，将电机2接口的OUT3与OUT2与小车的另一

个电机的正负极连接起来。然后将两边的接线端子，即供电正极（中间的接线端子为接地）连接TPYboard的

VIN，中间的接线端子即接地，连接TPYBoard的GND，In1-In4连接TPYBoard的X1，X2，Y1，Y2，通过X1,X2与

Y1,Y2的高低电平，来控制电机的转动，从而让小车前进，后退，向左，向右。

四、舵机

1、概述  

舵机也叫伺服电机，最早用于船舶上实现其转向功能，由于可以通过程序连续控制其转角，因而被广泛应用

智能小车以实现转向以及机器人各类关节运动中，如下图所示。

2、舵机的组成

一般来讲，舵机主要由以下几个部分组成，舵盘、减速齿轮组、位置反馈电位计、直流电机、控制电路等，

如下图所示。

舵机的输入线共有三条，红色中间，是电源线，一边灰色的是地线，这辆根线给舵机提供最基本的能源保

证，主要是电机的转动消耗。电源有两种规格，一是4.8V，一是6.0V，分别对应不同的转矩标准，即输出力

矩不同，6.0V对应的要大一些，具体看应用条件；另外一根线是控制信号线，Futaba的一般为白色，JR的一

般为桔黄色。另外要注意一点，SANWA的某些型号的舵机引线电源线在边上而不是中间，需要辨认。但记住红

色为电源，灰色为地线，剩下的一根为信号线，一般不会搞错。本实验中，舵机红色接TPYBoard v102+的VIN

引脚，灰色接TPYBoard v102+的GND引脚，剩下的橙色是信号线，接TPYBoard V102+的X3针脚。TPYBoard

 v102+ 的X1、X2、X3、X4为信号引脚。

3、舵机工作原理 

控制电路板接受来自信号线的控制信号，控制电机转动，电机带动一系列齿轮组，减速后传动至输出舵盘。

舵机的输出轴和位置反馈电位计是相连的，舵盘转动的同时，带动位置反馈电位计，电位计将输出一个电压

信号到控制电路板，进行反馈，然后控制电路板根据所在位置决定电机转动的方向和速度，从而达到目标停

止。其工作流程为：控制信号→控制电路板→电机转动→齿轮组减速→舵盘转动→位置反馈电位计→控制电

路板反馈。 

五、伺服电机的构造函数与方法

1、构造函数

pyb.Servo(id)

在这id为1-4，对应TPYBoard v102+的X1-X4。

2、方法

Servo.angle（[angle，time = 0 ]）

如果没有给出参数，则该函数返回当前角度。

如果给出参数，则该函数设置伺服角度：

angle 是以度数移动的角度。

time是达到指定角度所需的毫秒数。如果省略，则伺服器尽可能快地移动到其新位置。

Servo.speed([speed, time=0])

如果没有给出参数，则该函数返回当前速度。

如果给出参数，则该功能设置伺服的速度：

speed 是在-100到100之间变化的速度。

time是达到指定速度所需的毫秒数。如果省略，则伺服器尽可能快地加速。

Servo.pulse_width（[value ]）

如果没有给出参数，该函数返回当前的原始脉冲宽度值。

如果给出参数，则该函数设置原始脉冲宽度值。

Servo.calibration（[pulse_min，pulse_max，pulse_centre [，pulse_angle_90，pulse_speed_100 ]]）

如果没有给出参数，则该函数返回当前的校准数据，作为5元组。

如果给出参数，该功能设置时序校准：

pulse_min 是允许的最小脉冲宽度。

pulse_max 是允许的最大脉冲宽度。

pulse_centre 是对应于中心/零位置的脉冲宽度。

pulse_angle_90 是对应于90度的脉冲宽度。

pulse_speed_100 是对应于速度100的脉冲宽度。

六、实验要求

本实验为智能避障小车的实验，主要实现小车的避障功能，当前方距离过近的时候，TPYBoard v102+会控制

舵机转动超声波云台支架来判断前方、左前方、右前方的距离，从而控制小车向左转，向右转或者向前转，

实现避障的功能。

七、视频地址

优酷地址：

http://v.youku.com/v_show/id_XMjY2MTQ1NDA2MA==.html?spm=a2h0j.11185381.listitem_page1.5!4~A&qq-pf-to=pcqq.c2c

八、源代码

# main.py -- put your code here!
import pyb
from pyb import Pin
from pyb import Timer
from pyb import servo

x1 = Pin('X1', Pin.OUT_PP)
x2 = Pin('X2', Pin.OUT_PP)
y1 = Pin('Y1', Pin.OUT_PP)
y2 = Pin('Y2', Pin.OUT_PP)
Trig = Pin('X9',Pin.OUT_PP)
Echo = Pin('X10',Pin.IN)

num=0
flag=0
run=1
zuo=0
qian=0
you=0
distance=0
def start(t):
    global flag
    global num
    if(flag==0):
        num=0
    else:
        num=num+1
def stop(t):
    global run
    if(run==0):
        run=1
start1=Timer(1,freq=10000,callback=start)
stop1=Timer(4,freq=2,callback=stop)

#小车左转
def left():
    x1.high()
    x2.low()
    y1.high()
    y2.low()
#小车前进
def go():
    x1.high()
    x2.low()
    y1.low()
    y2.high()
#小车后退
def back():
    x1.low()
    x2.high()
    y1.high()
    y2.low()
#小车右转
def right():
    x1.low()
    x2.high()
    y1.low()
    y2.high()
#小车停止
def stop():
    x1.low()
    x2.low()
    y1.low()
    y2.low()
#控制舵机向右、向左、向前
def servo():
    global distance
    global zuo
    global you
    global qian
    servo3=pyb.Servo(3)
    #向右75旋转
    servo3.angle(-75,500)
    pyb.delay(1000)
    ceju()
    you=distance
    print('you',you)
    #向左转75度
    servo3.angle(75,500)
    pyb.delay(1000)
    ceju()
    zuo=distance
    print('zuo',distance)
    #转到0度
    servo3.angle(0,500)
    pyb.delay(1000)
    ceju()
    qian=distance
    print('qian',distance)
    #返回正前方、左前方、右前方的距离
    return qian,zuo,you
#测距方法
def  ceju():
    global flag
    global num
    global run
    global distance
    if(run==1):
        Trig.value(1)
        pyb.udelay(100)
        Trig.value(0)
        while(Echo.value()==0):
            Trig.value(1)
            pyb.udelay(100)
            Trig.value(0)
            flag=0
        if(Echo.value()==1):
            flag=1
            while(Echo.value()==1):           
                flag=1
        if(num!=0):
            #测距
            distance=num/10000*34299/2
            #print('Distance:')
            #print(distance,'cm')
            pyb.delay(500)
        flag=0
        run=0
    return distance
def main():
    global distance
    global zuo
    global you
    global qian
    servo3=pyb.Servo(3)
    servo3.angle(0,500)
    pyb.delay(1000)
    ceju()
    while 1==1:
        ceju()、
        #前方距离大于40cm前进
        if distance > 40:
            go()
            ceju()
            print('juli',distance)
        #前方距离小于40cm
        if distance <= 40:
            stop()
            back()
            pyb.delay(500)
            stop()
            servo()
            #如果右前方距离大于左前方
            if you>zuo:
                right()
                pyb.delay(400)
                ceju()
            #如果左前方距离大于右前方
            if zuo>you:
                left()
                pyb.delay(400)
                ceju()
            #如果所有方向距离全部小于15cm
            if zuo<15 and you< 15 and qian<15 :
                stop()
                back()
                pyb.delay(800)
                stop()
                ceju()
            #有一个方向距离小于5CM
            if zuo<5 or you <5 or qian<5:
                stop()
                back()
                pyb.delay(800)
                stop()
                ceju()
main()