尝试编写程序实现小车追踪色块同时躲避障碍
但是发现在蓝牙手机端输入指令后无反应
逐一测试之后初步判定问题出现在while TRUE循环中第二个if语句的条件
测试中,tts只播报“一”
我检查了根究 没发现错误,并且这段代码是贴过来的,源代码可以使用
求指出问题,谢谢
# main.py -- put your code here!
import pyb, utime, sensor, image
from pyb import Pin,UART,ExtInt,TTS,delay
#语音播报
#Create TTS UART Object
tts = TTS()
uart = UART("UAT")
# Initialize UART
uart.init(115200, bits=8, parity=None, stop=1, timeout=100)
# 开机启动延时
utime.sleep_ms(500)
# 点阵外设初始化
dot = pyb.DOTS()
# 点阵显示心形
dot.display(b'\x30\x78\x7C\x3E\x3E\x7C\x78\x30')
dot.show()
# OLED外设初始化
lcd = pyb.OLED()
lcd.fill(0)
# 蓝牙外设初始化
ble = pyb.UART("BLE")
ble.init(57600, bits=8, parity=None, stop=1, timeout=10)
# 获取蓝牙MAC地址并在OLED屏显示
ble.write('+++a') # 进入蓝牙配置模式
ble.read(10)
ble.write("AT+MAC?\r\n") # 获取蓝牙MAC地址
mac = ble.read(100)[7:19]
ble.write("AT+Z\r\n") # 退出蓝牙配置模式
ble.read(100) # 清空蓝牙初始信息
mac0 = mac[0:2] + ':' + mac[2:4] + ':' + mac[4:6] + ':' + mac[6:8] + ':' + mac[8:10] + ':' + mac[10:12]
mac1 = mac[0:2] + ':' + mac[2:4] + ':' + mac[4:6]
mac2 = mac[6:8] + ':' + mac[8:10] + ':' + mac[10:12]
lcd.qrcode(mac0, 1, 1, 1)
lcd.text('MAC:', 49, 2, 1)
lcd.text(mac1, 33, 12, 1)
lcd.text(mac2, 33, 22, 1)
lcd.show()
delay(2000)
lcd.fill(0)
lcd.text('recognizing',10,20,1)
lcd.show()
try:
# 电机驱动初始化
car = pyb.Vehicle()
car.run(0)
# 超声波传感器初始化
dist = pyb.Ultrasonic('F')
utime.sleep_ms(100)
dist.start()
except Exception: # 电机驱动初始化出错:未连接小车
lcd.fill(0)
lcd.text("Car", 36, 8, 1)
lcd.text("Disconnected", 2, 18, 1)
lcd.show()
while True:
print("Car Disconnected!")
utime.sleep_ms(1000)
# 中间变量定义
mode_run = -1
last_millis = utime.ticks_ms()
speed = 500
dot_flag = 0
rng_flag = 1
dir_rng = 1
thresholds = (10, 80, -70, -10, -0, 30) # 绿色阀值
tag_families = image.TAG16H5 # AprilTag类型定义
while True:
# 蓝牙数据处理
data = ble.read(10) # 蓝牙接收
if data != None:
tts.play("一")
delay(1000)
if len(data) == 6 and data[:5] == b"mode=":
tts.play("二")
mode_rcv = int(data[5:])
if mode_rcv < 0 or mode_rcv > 8:
tts.play("三")
mode_rcv = 0
if mode_rcv != mode_run: # 模式改变
tts.play("四")
mode_run = mode_rcv
#car.run(0)
ble.write("mode=%d" % mode_run)
#ble.write("speed=%d" % speed)
tts.play("成功运行")
# 运动控制
if mode_run == 0: # 停止
car.run(0)
#mode_run = -1 # 避免反复执行相同动作
elif mode_run == 1: # 前进
car.run(speed)
elif mode_run == 2: # 后退
car.run(-speed)
elif mode_run == 3: # 左转
car.run(-speed, speed)
elif mode_run == 4: # 右转
car.run(speed, -speed)
elif mode_run == 5: # 自动避障
d = dist.get()
lcd.fill(0)
show = "Dist:%dcm" % d
lcd.text(show, 10, 10, 1)
lcd.show()
if d <= 35:
if rng_flag == 1:
rng_flag = 0
dir_rng = (pyb.rng() % 2) * 2 - 1
car.run(speed * dir_rng, -speed * dir_rng)
else:
rng_flag = 1
car.run(speed)
dist.start()
utime.sleep_ms(100)
elif mode_run == 6: # 颜色跟踪
img = sensor.snapshot() # 获取摄像头快照
valid = False
# 寻找绿色块
for blob in img.find_blobs([thresholds], pixels_threshold=200, area_threshold=200, merge=True):
if blob.pixels() > 500 and blob.pixels() < 18000: # 排除过大或过小的色块,用来判断远近
diff = blob.cx() - 160
if diff > 60:
car.run(-speed, speed)
elif diff < -60:
car.run(speed, -speed)
else:
car.run(speed)
else:
car.run(0)
valid = True
else:
if valid == False:
car.run(0)
elif mode_run == 7: # 二维码跟踪
img = sensor.snapshot() # 获取摄像头快照
valid = False
# 寻找AprilTag码
for tag in img.find_apriltags(families=tag_families):
if tag.w() < 55:
diff = tag.cx() - 80
if diff > 30:
car.run(-speed, speed)
elif diff < -30:
car.run(speed, -speed)
else:
car.run(speed)
elif tag.w() >= 55:
car.run(0)
else:
car.run(0)
show = "ID:%d" % tag.id()
lcd.fill(0)
lcd.text(show, 10, 10, 1)
lcd.show()
valid = True
else:
if valid == False:
car.run(0)
elif mode_run == 8:
tts.play("进入自动驾驶模式")
# 加载颜色跟踪
sensor.reset() # 摄像头复位初始化
sensor.set_pixformat(sensor.RGB565) # 设置帧格式
sensor.set_framesize(sensor.QVGA) # 设置帧大小(QVGA:320*240,QQVGA:160*120,QQQVGA:80*60,QQQQVGA:40*30)
sensor.skip_frames(time=2000) # 设置跳帧,间隔2s一帧
sensor.set_auto_gain(False) # 自动增益,颜色追踪时,自动增益关闭
sensor.set_auto_whitebal(False) # 自动白平衡,颜色追踪时,自动增益关闭
img = sensor.snapshot() # 获取摄像头快照
valid = False
# 寻找绿色块
for blob in img.find_blobs([thresholds], pixels_threshold=200, area_threshold=200, merge=True):
if blob.pixels() > 500 and blob.pixels() < 18000: # 排除过大或过小的色块,用来判断远近
diff = blob.cx() - 160
if diff > 60:
car.run(-speed, speed)
elif diff < -60:
car.run(speed, -speed)
else:
car.run(speed)
# 自动避障
d = dist.get()
lcd.fill(0)
show = "Dist:%dcm" % d
lcd.text(show, 10, 10, 1)
lcd.show()
if d <= 35:
if rng_flag == 1:
rng_flag = 0
dir_rng = (pyb.rng() % 2) * 2 - 1
car.run(speed * dir_rng, -speed * dir_rng)
else:
rng_flag = 1
car.run(speed)
dist.start()
utime.sleep_ms(100)
ble.write("mode=%d" % mode_run)
else:
car.run(0)
valid = True
else:
if valid == False:
car.run(0)
tts.play("目标追踪完成")
elif data[:6] == b'speed=':
speed = int(data[6:])
if speed < 400:
speed = 400
elif speed > 999:
speed = 999
# 作为遥控车时避免反复执行相同动作
if mode_run >= 0 and mode_run <= 4:
mode_run = -1
# 点阵心跳(每500ms变化一次)
if utime.ticks_diff(utime.ticks_ms(), last_millis) > 500:
last_millis = utime.ticks_ms()
if dot_flag == 0:
dot_flag = 1
dot.display(b'\x30\x78\x7C\x3E\x3E\x7C\x78\x30')
else:
dot_flag = 0
dot.display(b'\x00\x30\x38\x1c\x1c\x38\x30\x00')
dot.show()