实验器材

1、TPYboard V102板  1块
2、电机驱动模块L298N   1个
3、电机 2块
4、小车底盘 1个
5、超声波模块 1个
6、舵机 1个
7、SG90舵机支架 1个
8、超声波云台支架 1个

# 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()