933 lines
36 KiB
Python
933 lines
36 KiB
Python
|
# -*- coding:utf-8 -*-
|
|||
|
|
# @Author len
|
|||
|
|
# @Create 2023/11/23 11:28
|
|||
|
|
|
|||
|
|
import math
|
|||
|
|
import sensor, image, time, lcd
|
|||
|
|
import binascii
|
|||
|
|
from Maix import GPIO
|
|||
|
|
from machine import Timer, PWM, UART, Timer
|
|||
|
|
from fpioa_manager import fm
|
|||
|
|
import KPU as kpu
|
|||
|
|
|
|||
|
|
|
|||
|
|
class Mainlen():
|
|||
|
|
def __init__(self):
|
|||
|
|
'''初始化摄像头和 LCD 显示屏'''
|
|||
|
|
lcd.init() # lcd初始化
|
|||
|
|
|
|||
|
|
self.canera_init() # 摄像头初始化
|
|||
|
|
# sensor.set_auto_gain(0, gain_db=17) # 设置摄像头的自动增益功能
|
|||
|
|
|
|||
|
|
# 映射串口引脚
|
|||
|
|
fm.register(6, fm.fpioa.UART1_RX, force=True)
|
|||
|
|
fm.register(7, fm.fpioa.UART1_TX, force=True)
|
|||
|
|
|
|||
|
|
# 初始化串口
|
|||
|
|
self.uart = UART(UART.UART1, 115200, read_buf_len=4096)
|
|||
|
|
|
|||
|
|
# 循迹
|
|||
|
|
# --------------感光芯片配置 START -------------------
|
|||
|
|
self.IMG_WIDTH = 240
|
|||
|
|
self.IMG_HEIGHT = 320
|
|||
|
|
# 直线灰度图颜色阈值
|
|||
|
|
self.LINE_COLOR_THRESHOLD = [(0, 60)] # 找黑色
|
|||
|
|
self.LINE_COLOR_BAISE = [(60, 255)] # 找白色
|
|||
|
|
self.ROIS = { # 找黑色
|
|||
|
|
# 'left': (0, 0, 320, 50), # 纵向取样-左侧
|
|||
|
|
# 'right': (0, 190, 320, 50), # 纵向取样-右侧
|
|||
|
|
'left': (0, 0, 180, 50), # 纵向取样-左侧
|
|||
|
|
'right': (0, 190, 180, 50), # 纵向取样-右侧
|
|||
|
|
'up': (240, 0, 80, 240), # 横向取样-上方
|
|||
|
|
'middle_up': (160, 0, 80, 240), # 横向取样-中上
|
|||
|
|
'middle_down': (80, 0, 80, 240), # 横向取样-中下
|
|||
|
|
'down': (0, 0, 80, 240), # 横向取样-下方
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
# 红黄绿的阈值
|
|||
|
|
# 颜色识别阈值 (L Min, L Max, A Min, A Max, B Min, B Max) LAB模型
|
|||
|
|
# 下列阈值元组用来识别 红、绿、蓝三种颜色，可通过调整数据阈值完成更多颜色的识别。
|
|||
|
|
# self.thresholds = [(59, 100, 40, 127, 5, 127), # 红色阈值
|
|||
|
|
# (90, 100, -5, 2, -4, 20), # 黄色阈值
|
|||
|
|
# (87, 100, -59, 127, -10, 127),] # 绿色阈值
|
|||
|
|
|
|||
|
|
self.thresholds = [(56, 99, 13, 63, -18, 37, "红色"), # 红色阈值
|
|||
|
|
(69, 100, -7, 14, 1, 46, "黄色"), # 黄色阈值
|
|||
|
|
(55, 100, -51, -16, -9, 83, "绿色")] # 绿色阈值
|
|||
|
|
|
|||
|
|
self.is_need_send_data = False # 是否需要发送数据的信号标志
|
|||
|
|
|
|||
|
|
self.QRFlag = 0
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 主函数 run
|
|||
|
|
def startMain(self):
|
|||
|
|
Flag_track = False # 循迹标识
|
|||
|
|
Flag_qr = False # 二维码标识
|
|||
|
|
Flag_qr_2 = False # 标识B
|
|||
|
|
Flag_qr_3 = False # 二维码3
|
|||
|
|
Flag_light = False # 交通灯标识
|
|||
|
|
Flag_Camera_wheel = False # 摄像头转弯
|
|||
|
|
while True:
|
|||
|
|
data = self.uart.read(8)
|
|||
|
|
# print(data)
|
|||
|
|
# print(len(data))
|
|||
|
|
# if (len(data) >= 8):
|
|||
|
|
if data is not None:
|
|||
|
|
if (data[1] == 0x02) and (data[7] == 0xBB) and self.verify_checksum(data):
|
|||
|
|
# 巡线与控制舵机
|
|||
|
|
if data[2] == 0x91:
|
|||
|
|
if data[3] == 0x01: # 启动循迹
|
|||
|
|
sensor.set_pixformat(sensor.GRAYSCALE) # 设置像素格式为灰色
|
|||
|
|
Flag_track = True
|
|||
|
|
print("开始循迹")
|
|||
|
|
|
|||
|
|
elif data[3] == 0x02: # 停止循迹
|
|||
|
|
sensor.set_pixformat(sensor.RGB565) # 设置像素格式为彩色 RGB565
|
|||
|
|
Flag_track = False
|
|||
|
|
print("停止循迹")
|
|||
|
|
|
|||
|
|
elif data[3] == 0x03: # 调整舵机
|
|||
|
|
print("调整舵机")
|
|||
|
|
self.Servo(data)
|
|||
|
|
|
|||
|
|
elif data[3] == 0x04: # 启动摄像头转弯
|
|||
|
|
print("摄像头转弯")
|
|||
|
|
Flag_Camera_wheel = True
|
|||
|
|
|
|||
|
|
elif data[3] == 0x05: # 停止摄像头转弯
|
|||
|
|
print("停止摄像头转弯")
|
|||
|
|
Flag_Camera_wheel = False
|
|||
|
|
|
|||
|
|
else:
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
# 识别任务
|
|||
|
|
elif data[2] == 0x92:
|
|||
|
|
if data[3] == 0x01: # 识别二维码
|
|||
|
|
sensor.set_pixformat(sensor.RGB565) # 设置像素格式为彩色 RGB565
|
|||
|
|
Flag_qr = True
|
|||
|
|
print("开始识别二维码")
|
|||
|
|
|
|||
|
|
elif data[3] == 0x02: # 停止识别二维码
|
|||
|
|
Flag_qr = False
|
|||
|
|
print("停止识别二维码")
|
|||
|
|
|
|||
|
|
elif data[3] == 0x03: # 识别交通灯
|
|||
|
|
sensor.set_pixformat(sensor.RGB565) # 设置像素格式为彩色 RGB565
|
|||
|
|
Flag_light = True
|
|||
|
|
print("开始识别交通灯")
|
|||
|
|
|
|||
|
|
elif data[3] == 0x04: # 停止识别交通灯
|
|||
|
|
Flag_light = False
|
|||
|
|
print("停止识别交通灯")
|
|||
|
|
|
|||
|
|
elif data[3] == 0x06: # 调整摄像头阈值
|
|||
|
|
print("调整摄像头阈值")
|
|||
|
|
self.canera_ash()
|
|||
|
|
|
|||
|
|
elif data[3] == 0x09:
|
|||
|
|
print("开始识别第二个二维码")
|
|||
|
|
Flag_qr_2 = True
|
|||
|
|
|
|||
|
|
elif data[3] == 0x10:
|
|||
|
|
print("停止识别第二个二维码")
|
|||
|
|
Flag_qr_2 = False
|
|||
|
|
elif data[3] == 0x12:
|
|||
|
|
print("开始识别第三个二维码")
|
|||
|
|
Flag_qr_3 = True
|
|||
|
|
|
|||
|
|
elif data[3] == 0x13:
|
|||
|
|
print("停止识别第三个二维码")
|
|||
|
|
Flag_qr_3 = False
|
|||
|
|
else:
|
|||
|
|
pass
|
|||
|
|
img = sensor.snapshot() # 获取图像
|
|||
|
|
if Flag_track: # 循迹
|
|||
|
|
print("循迹")
|
|||
|
|
self.tracking(img)
|
|||
|
|
|
|||
|
|
elif Flag_Camera_wheel: # 摄像头转弯
|
|||
|
|
print("摄像头转弯")
|
|||
|
|
self.Camera_wheel(img)
|
|||
|
|
|
|||
|
|
elif Flag_qr and (self.QRFlag <3): # 二维码
|
|||
|
|
print("识别二维码")
|
|||
|
|
self.discem_QR(img)
|
|||
|
|
Flag_qr = False
|
|||
|
|
|
|||
|
|
elif Flag_light: # 红绿灯
|
|||
|
|
print("识别红绿灯")
|
|||
|
|
self.discem_light()
|
|||
|
|
# Flag_light = False
|
|||
|
|
|
|||
|
|
elif Flag_qr_2:
|
|||
|
|
print("识别第二个二维码")
|
|||
|
|
self.discem_QR_2(img)
|
|||
|
|
Flag_qr_2 = False
|
|||
|
|
|
|||
|
|
elif Flag_qr_3:
|
|||
|
|
print("识别第三个二维码")
|
|||
|
|
self.discem_QR_3(img)
|
|||
|
|
Flag_qr_3 = False
|
|||
|
|
|
|||
|
|
lcd.display(img) # 在LCD显示
|
|||
|
|
|
|||
|
|
# 摄像头转弯
|
|||
|
|
def Camera_wheel(self, img):
|
|||
|
|
canvas = img.copy()
|
|||
|
|
roi_blobs_result = {} # 在各个ROI中寻找色块的结果记录
|
|||
|
|
for roi_direct in self.ROIS.keys():
|
|||
|
|
roi_blobs_result[roi_direct] = {
|
|||
|
|
'cx': 0,
|
|||
|
|
'cy': 0,
|
|||
|
|
'w': 0,
|
|||
|
|
'blob_flag': False
|
|||
|
|
}
|
|||
|
|
for roi_direct, roi in self.ROIS.items():
|
|||
|
|
blobs = canvas.find_blobs(self.LINE_COLOR_THRESHOLD, roi=roi, merge=True)
|
|||
|
|
if len(blobs) != 0:
|
|||
|
|
largest_blob = max(blobs, key=lambda b: b.pixels())
|
|||
|
|
if largest_blob.area() > 1000:
|
|||
|
|
roi_blobs_result[roi_direct]['cx'] = largest_blob.cy()
|
|||
|
|
roi_blobs_result[roi_direct]['cy'] = largest_blob.cx()
|
|||
|
|
roi_blobs_result[roi_direct]['w'] = largest_blob.h()
|
|||
|
|
roi_blobs_result[roi_direct]['blob_flag'] = True
|
|||
|
|
x, y, width, height = largest_blob[:4]
|
|||
|
|
img.draw_rectangle((x, y, width, height), color=(255))
|
|||
|
|
else:
|
|||
|
|
# blobs=canvas.find_blobs(LINE_COLOR_THRESHOLD, roi=roi, merge=True, pixels_area=10)
|
|||
|
|
continue
|
|||
|
|
|
|||
|
|
blobs_zuo = canvas.find_blobs(self.LINE_COLOR_THRESHOLD, roi=(0, 0, 180, 50), merge=True) # 车载摄像头屏幕左下部找黑色
|
|||
|
|
blobs_you = canvas.find_blobs(self.LINE_COLOR_THRESHOLD, roi=(0, 190, 180, 50), merge=True) # 车载摄像头屏幕右下部找黑色
|
|||
|
|
print(blobs_zuo)
|
|||
|
|
print(blobs_you)
|
|||
|
|
if len(blobs_zuo) != 0:
|
|||
|
|
zuo = blobs_zuo[0][4]
|
|||
|
|
else:
|
|||
|
|
zuo = 0
|
|||
|
|
if len(blobs_you) != 0:
|
|||
|
|
you = blobs_you[0][4]
|
|||
|
|
else:
|
|||
|
|
you = 0
|
|||
|
|
print(zuo, you)
|
|||
|
|
if zuo >= 8000 or you >= 8000:
|
|||
|
|
print("黑线")
|
|||
|
|
wire = 0x01
|
|||
|
|
else:
|
|||
|
|
print("白线")
|
|||
|
|
wire = 0x00
|
|||
|
|
# print()
|
|||
|
|
self.uart.write(bytes([0x55]))
|
|||
|
|
self.uart.write(bytes([0x02]))
|
|||
|
|
self.uart.write(bytes([0x93]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0x03])) # 线
|
|||
|
|
self.uart.write(bytes([wire])) # 黑白线
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0x03]))
|
|||
|
|
self.uart.write(bytes([0x03]))
|
|||
|
|
self.uart.write(bytes([0xbb]))
|
|||
|
|
|
|||
|
|
# 初始化摄像头阈值
|
|||
|
|
def canera_init(self):
|
|||
|
|
# 摄像头模块初始化
|
|||
|
|
sensor.reset() # 复位和初始化摄像头
|
|||
|
|
sensor.set_pixformat(sensor.RGB565) # 设置像素格式为彩色 RGB565
|
|||
|
|
# sensor.set_pixformat(sensor.GRAYSCALE) # 设置像素格式为灰色
|
|||
|
|
sensor.set_framesize(sensor.QVGA) # 设置帧大小为QVGA（320×240）
|
|||
|
|
sensor.set_vflip(1) # 后置模式
|
|||
|
|
sensor.skip_frames(30) # # 跳过前30帧
|
|||
|
|
|
|||
|
|
# 调整摄像头阈值
|
|||
|
|
def canera_ash(self):
|
|||
|
|
sensor.reset() # 复位和初始化摄像头
|
|||
|
|
sensor.set_vflip(1) # 将摄像头设置成后置方式（所见即所得）
|
|||
|
|
sensor.set_pixformat(sensor.RGB565) # 设置像素格式为彩色 RGB565
|
|||
|
|
# sensor.set_pixformat(sensor.GRAYSCALE) # 设置像素格式为灰色
|
|||
|
|
sensor.set_framesize(sensor.QVGA) # 设置帧大小为 QVGA (320x240)
|
|||
|
|
# sensor.set_windowing((224, 224)) # 设置摄像头的窗口大小
|
|||
|
|
sensor.set_auto_gain(False)
|
|||
|
|
sensor.set_auto_whitebal(False)
|
|||
|
|
sensor.set_auto_gain(0, 0)
|
|||
|
|
sensor.skip_frames(time=200) # 等待设置生效
|
|||
|
|
|
|||
|
|
# 红绿灯
|
|||
|
|
def discem_light(self):
|
|||
|
|
# 初始化
|
|||
|
|
results = []
|
|||
|
|
start_time = time.time()
|
|||
|
|
|
|||
|
|
while time.time() - start_time < 3: # 循环持续三秒
|
|||
|
|
img = sensor.snapshot()
|
|||
|
|
img = img.crop((160, 0, 80, 224))
|
|||
|
|
# print(img.width()) # 320
|
|||
|
|
# print(img.height()) #240
|
|||
|
|
# img = img.crop((0, 0, 160, 224))
|
|||
|
|
max_blob = None
|
|||
|
|
max_blob_size = 0
|
|||
|
|
max_blob_color = ""
|
|||
|
|
max_blob_color_index = 0
|
|||
|
|
# 找圆
|
|||
|
|
# circles = img.find_circles(threshold=3500, x_margin=10, y_margin=10, r_margin=10,
|
|||
|
|
# r_min=2, r_max=100, r_step=2)
|
|||
|
|
for index, threshold in enumerate(self.thresholds):
|
|||
|
|
# 查找每种颜色的色块
|
|||
|
|
blobs = img.find_blobs([threshold[:-1]], pixels_threshold=150, area_threshold=150, merge=True)
|
|||
|
|
if blobs:
|
|||
|
|
# 找到最大的色块
|
|||
|
|
blob = max(blobs, key=lambda b: b.pixels())
|
|||
|
|
if blob.pixels() > max_blob_size:
|
|||
|
|
max_blob = blob
|
|||
|
|
max_blob_size = blob.pixels()
|
|||
|
|
max_blob_color = threshold[-1] # 颜色标签
|
|||
|
|
max_blob_color_index = index
|
|||
|
|
|
|||
|
|
# print(circles)
|
|||
|
|
# # 画圆
|
|||
|
|
# for c in circles:
|
|||
|
|
# area = (c.x() - c.r(), c.y() - c.r(), 2 * c.r(), 2 * c.r())
|
|||
|
|
# img.draw_rectangle(area, color=(255, 255, 255))
|
|||
|
|
# 绘制最大色块的矩形和中心十字，并输出颜色
|
|||
|
|
if max_blob:
|
|||
|
|
results.append((max_blob_color, max_blob_color_index)) # 存储识别结果
|
|||
|
|
img.draw_rectangle(max_blob[0:4])
|
|||
|
|
img.draw_cross(max_blob[5], max_blob[6])
|
|||
|
|
print("最大色块的颜色是:", max_blob_color, max_blob_color_index)
|
|||
|
|
# 初始化计数字典
|
|||
|
|
color_count = {}
|
|||
|
|
|
|||
|
|
for color, index in results:
|
|||
|
|
if (color, index) in color_count:
|
|||
|
|
color_count[(color, index)] += 1
|
|||
|
|
else:
|
|||
|
|
color_count[(color, index)] = 1
|
|||
|
|
|
|||
|
|
# 找出出现次数最多的颜色和下标
|
|||
|
|
if color_count:
|
|||
|
|
most_common_color, most_common_index = max(color_count, key=color_count.get)
|
|||
|
|
print("出现最多的颜色:", most_common_color, "在下标位置:", most_common_index)
|
|||
|
|
else:
|
|||
|
|
most_common_index = 1
|
|||
|
|
print("没有识别到有效的颜色块")
|
|||
|
|
|
|||
|
|
# 串口发送交通灯信息
|
|||
|
|
# send_data = [
|
|||
|
|
# 0x55,
|
|||
|
|
# 0x02,
|
|||
|
|
# 0x92,
|
|||
|
|
# 0x03,
|
|||
|
|
# most_common_index,
|
|||
|
|
# 0xbb]
|
|||
|
|
# print("红绿灯指令", send_data, "******************", bytes(send_data))
|
|||
|
|
# self.UsartSend(bytes(send_data))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0x55]))
|
|||
|
|
self.uart.write(bytes([0x02]))
|
|||
|
|
self.uart.write(bytes([0x92]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0x03])) # 红绿灯
|
|||
|
|
self.uart.write(bytes([most_common_index])) # 红绿灯结果
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0x03]))
|
|||
|
|
self.uart.write(bytes([0x03]))
|
|||
|
|
self.uart.write(bytes([0xbb]))
|
|||
|
|
|
|||
|
|
self.canera_init() # 恢复摄像头
|
|||
|
|
|
|||
|
|
# 二维码
|
|||
|
|
def discem_QR(self, img):
|
|||
|
|
# self.Tise_servo(10)
|
|||
|
|
res_QR = img.find_qrcodes() # 寻找二维码
|
|||
|
|
timeflag = 0
|
|||
|
|
while timeflag < 30:
|
|||
|
|
img = sensor.snapshot() # 获取图像
|
|||
|
|
TS_QR = img.find_qrcodes() # 再次寻找二维码
|
|||
|
|
for qr in TS_QR:
|
|||
|
|
if all(qr.payload() != existing_qr.payload() for existing_qr in res_QR):
|
|||
|
|
res_QR.append(qr)
|
|||
|
|
timeflag += 1
|
|||
|
|
time.sleep(0.001)
|
|||
|
|
# print(timeflag)
|
|||
|
|
print(len(res_QR))
|
|||
|
|
|
|||
|
|
if len(res_QR): # 识别到
|
|||
|
|
for res in range(len(res_QR)):
|
|||
|
|
count = 0
|
|||
|
|
if_end = 0x01
|
|||
|
|
result = res_QR[res].payload()
|
|||
|
|
print(result)
|
|||
|
|
result = self.get_test_inside_brackets(result)
|
|||
|
|
|
|||
|
|
if self.if_formula(result): # 公式
|
|||
|
|
count = 2
|
|||
|
|
else: # 车牌
|
|||
|
|
result = self.extract_numbers_and_letters_combined(result)
|
|||
|
|
count = 1
|
|||
|
|
|
|||
|
|
|
|||
|
|
if res == len(res_QR) - 1:
|
|||
|
|
if_end = 0x00
|
|||
|
|
# if_end = 0x00
|
|||
|
|
print("count: ", count)
|
|||
|
|
print("if_end: ", if_end)
|
|||
|
|
|
|||
|
|
# 串口发送二维码信息
|
|||
|
|
self.uart.write(bytes([0x55]))
|
|||
|
|
self.uart.write(bytes([0x02]))
|
|||
|
|
self.uart.write(bytes([0x92]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0x06]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([count]))
|
|||
|
|
self.uart.write(bytes([if_end]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([len(result)]))
|
|||
|
|
for qr_data in result:
|
|||
|
|
self.uart.write(bytes([ord(qr_data)]))
|
|||
|
|
print(bytes([ord(qr_data)]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0xbb]))
|
|||
|
|
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
|
|||
|
|
else: # 未识别
|
|||
|
|
self.uart.write(bytes([0x55]))
|
|||
|
|
self.uart.write(bytes([0x02]))
|
|||
|
|
self.uart.write(bytes([0x92]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0x06]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0xff]))
|
|||
|
|
self.uart.write(bytes([0x00]))
|
|||
|
|
self.uart.write(bytes([0x00]))
|
|||
|
|
self.uart.write(bytes([0xbb]))
|
|||
|
|
|
|||
|
|
# time.sleep(1)
|
|||
|
|
|
|||
|
|
def discem_QR_2(self, img):
|
|||
|
|
# self.Tise_servo(10)
|
|||
|
|
res_QR = img.find_qrcodes() # 寻找二维码
|
|||
|
|
timeflag = 0
|
|||
|
|
while timeflag < 30:
|
|||
|
|
img = sensor.snapshot() # 获取图像
|
|||
|
|
TS_QR = img.find_qrcodes() # 再次寻找二维码
|
|||
|
|
for qr in TS_QR:
|
|||
|
|
if all(qr.payload() != existing_qr.payload() for existing_qr in res_QR):
|
|||
|
|
res_QR.append(qr)
|
|||
|
|
# print(qr)
|
|||
|
|
timeflag += 1
|
|||
|
|
time.sleep(0.001)
|
|||
|
|
# print(timeflag)
|
|||
|
|
print("QRnum:" , len(res_QR))
|
|||
|
|
|
|||
|
|
if len(res_QR) == 2:
|
|||
|
|
qr_code_a = res_QR[0].payload()
|
|||
|
|
qr_code_b = res_QR[1].payload()
|
|||
|
|
ascii_codes = self.qr_240606_2(qr_code_a, qr_code_b) # 最大子串
|
|||
|
|
|
|||
|
|
print(ascii_codes)
|
|||
|
|
# # 计算前6个字符的ASCII码
|
|||
|
|
# ascii_codes = [hex(ord(c)) for c in sorted_combined[:6]]
|
|||
|
|
# print(ascii_codes)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 串口发送二维码信息
|
|||
|
|
self.uart.write(bytes([0x55]))
|
|||
|
|
self.uart.write(bytes([0x02]))
|
|||
|
|
self.uart.write(bytes([0x92]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0x07]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0x01]))
|
|||
|
|
# self.uart.write(bytes([if_end]))
|
|||
|
|
#
|
|||
|
|
# self.uart.write(bytes([len(result_list)]))
|
|||
|
|
for qr_data in ascii_codes:
|
|||
|
|
self.uart.write(bytes([ord(qr_data)]))
|
|||
|
|
print(bytes([ord(qr_data)]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0xbb]))
|
|||
|
|
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
# for res in range(len(res_QR)):
|
|||
|
|
# count = 0
|
|||
|
|
# if_end = 0x01
|
|||
|
|
# result = res_QR[res].payload()
|
|||
|
|
# print(result)
|
|||
|
|
#
|
|||
|
|
# if len(result) == 8: # 车牌
|
|||
|
|
# result = self.extract_numbers_and_letters_combined(result)
|
|||
|
|
# count = 1
|
|||
|
|
#
|
|||
|
|
# elif self.if_formula(result): # 公式
|
|||
|
|
# count = 2
|
|||
|
|
# # continue
|
|||
|
|
#
|
|||
|
|
# if res == len(res_QR) - 1:
|
|||
|
|
# if_end = 0x00
|
|||
|
|
# # if_end = 0x00
|
|||
|
|
# print("count: ", count)
|
|||
|
|
# print("if_end: ", if_end)
|
|||
|
|
|
|||
|
|
else: # 未识别
|
|||
|
|
self.uart.write(bytes([0x55]))
|
|||
|
|
self.uart.write(bytes([0x02]))
|
|||
|
|
self.uart.write(bytes([0x92]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0x07]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0xff]))
|
|||
|
|
self.uart.write(bytes([0x00]))
|
|||
|
|
self.uart.write(bytes([0x00]))
|
|||
|
|
self.uart.write(bytes([0xbb]))
|
|||
|
|
|
|||
|
|
# time.sleep(1)
|
|||
|
|
|
|||
|
|
def discem_QR_3(self, img):
|
|||
|
|
# self.Tise_servo(10)
|
|||
|
|
res_QR = img.find_qrcodes() # 寻找二维码
|
|||
|
|
timeflag = 0
|
|||
|
|
while timeflag < 30:
|
|||
|
|
img = sensor.snapshot() # 获取图像
|
|||
|
|
TS_QR = img.find_qrcodes() # 再次寻找二维码
|
|||
|
|
for qr in TS_QR:
|
|||
|
|
if all(qr.payload() != existing_qr.payload() for existing_qr in res_QR):
|
|||
|
|
res_QR.append(qr)
|
|||
|
|
timeflag += 1
|
|||
|
|
time.sleep(0.001)
|
|||
|
|
# print(timeflag)
|
|||
|
|
print(len(res_QR))
|
|||
|
|
|
|||
|
|
if len(res_QR) == 2:
|
|||
|
|
qr_code_a = res_QR[0].payload()
|
|||
|
|
qr_code_b = res_QR[1].payload()
|
|||
|
|
ascii_codes = self.qr_240606(qr_code_a, qr_code_b) # 最大子串
|
|||
|
|
|
|||
|
|
print(ascii_codes)
|
|||
|
|
|
|||
|
|
# 串口发送二维码信息
|
|||
|
|
self.uart.write(bytes([0x55]))
|
|||
|
|
self.uart.write(bytes([0x02]))
|
|||
|
|
self.uart.write(bytes([0x92]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0x08]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0x01]))
|
|||
|
|
# self.uart.write(bytes([if_end]))
|
|||
|
|
#
|
|||
|
|
# self.uart.write(bytes([len(result_list)]))
|
|||
|
|
for qr_data in ascii_codes:
|
|||
|
|
self.uart.write(bytes([ord(qr_data)]))
|
|||
|
|
print(bytes([ord(qr_data)]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0xbb]))
|
|||
|
|
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
else: # 未识别
|
|||
|
|
self.uart.write(bytes([0x55]))
|
|||
|
|
self.uart.write(bytes([0x02]))
|
|||
|
|
self.uart.write(bytes([0x92]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0x08]))
|
|||
|
|
|
|||
|
|
self.uart.write(bytes([0xff]))
|
|||
|
|
self.uart.write(bytes([0x00]))
|
|||
|
|
self.uart.write(bytes([0x00]))
|
|||
|
|
self.uart.write(bytes([0xbb]))
|
|||
|
|
|
|||
|
|
# time.sleep(1)
|
|||
|
|
|
|||
|
|
def qr_240920(self, str1, str2):# 提取尖括号内的数据，数字和字母， 公式
|
|||
|
|
result = ""
|
|||
|
|
str1 = self.get_test_inside_brackets(str1) # 尖括号内的数据
|
|||
|
|
str2 = self.get_test_inside_brackets(str2) # 尖括号内的数据
|
|||
|
|
|
|||
|
|
|
|||
|
|
def qr_240606(self, str1, str2):
|
|||
|
|
result = ""
|
|||
|
|
ch1 = self.get_test_outside_brackets(str1)
|
|||
|
|
ch2 = self.get_test_outside_brackets(str2)
|
|||
|
|
|
|||
|
|
str1 = self.get_test_inside_brackets(str1)
|
|||
|
|
str2 = self.get_test_inside_brackets(str2)
|
|||
|
|
# print(str1, str2)
|
|||
|
|
if ch1 == 'A':
|
|||
|
|
result = self.compare_strings(str1, str2)
|
|||
|
|
elif ch2 == 'A':
|
|||
|
|
result = self.compare_strings(str2, str1)
|
|||
|
|
|
|||
|
|
return result
|
|||
|
|
|
|||
|
|
# 比较两个字符串的内容，存第一个二维码的不同
|
|||
|
|
def compare_strings(self, str1, str2):
|
|||
|
|
result1 = ""
|
|||
|
|
result2 = ""
|
|||
|
|
for i in range(len(str1)):
|
|||
|
|
if str1[i] != str2[i]:
|
|||
|
|
result1 += str1[i]
|
|||
|
|
result2 += str2[i]
|
|||
|
|
return result1 + result2
|
|||
|
|
|
|||
|
|
# 尖括号外的数据
|
|||
|
|
def get_test_outside_brackets(self, qrstring):
|
|||
|
|
# 提取尖括号外的内容
|
|||
|
|
start_index = qrstring.find('<')
|
|||
|
|
end_index = qrstring.find('>')
|
|||
|
|
# 如果尖括号存在，切割尖括号外的内容
|
|||
|
|
if start_index != -1 and end_index != -1:
|
|||
|
|
return qrstring[:start_index] + qrstring[end_index + 1:]
|
|||
|
|
else:
|
|||
|
|
# 如果尖括号不存在，返回原字符串
|
|||
|
|
return qrstring
|
|||
|
|
|
|||
|
|
# 尖括号内的数据
|
|||
|
|
def get_test_inside_brackets(self, qrstring):
|
|||
|
|
# 提取尖括号内的内容
|
|||
|
|
start_index = qrstring.find('<')
|
|||
|
|
end_index = qrstring.find('>')
|
|||
|
|
# 如果尖括号存在，切割尖括号内的内容
|
|||
|
|
if start_index != -1 and end_index != -1:
|
|||
|
|
return qrstring[start_index + 1:end_index]
|
|||
|
|
else:
|
|||
|
|
# 如果尖括号不存在，返回空字符串
|
|||
|
|
return ""
|
|||
|
|
|
|||
|
|
def qr_240606_2(self, qr_code_a, qr_code_b):
|
|||
|
|
longest_substring = self.find_longest_common_substring(qr_code_a, qr_code_b) # 最大子串
|
|||
|
|
# print("最大子串:", longest_substring)
|
|||
|
|
# 移除最大子字符串
|
|||
|
|
remaining_a = qr_code_a.replace(longest_substring, "", 1)
|
|||
|
|
remaining_b = qr_code_b.replace(longest_substring, "", 1)
|
|||
|
|
# 拼接并排序
|
|||
|
|
combined = remaining_a + remaining_b
|
|||
|
|
sorted_combined = ''.join(sorted(combined))
|
|||
|
|
# 计算前6个字符的ASCII码
|
|||
|
|
# ascii_codes = [hex(ord(c)) for c in sorted_combined[:6]]
|
|||
|
|
# print(ascii_codes)
|
|||
|
|
ascii_codes = sorted_combined[:6]
|
|||
|
|
# print(ascii_codes)
|
|||
|
|
return ascii_codes
|
|||
|
|
|
|||
|
|
# 提取数字字符
|
|||
|
|
def extract_numbers_without_isdigit(self, text):
|
|||
|
|
numbers_str = ''
|
|||
|
|
for char in text:
|
|||
|
|
# 检查字符是否是数字（0-9）
|
|||
|
|
if '0' <= char <= '9':
|
|||
|
|
numbers_str += char
|
|||
|
|
return numbers_str
|
|||
|
|
|
|||
|
|
# 提取字母字符
|
|||
|
|
def extract_letters_without_isalpha(self, text):
|
|||
|
|
letters_str = ''
|
|||
|
|
for char in text:
|
|||
|
|
# 检查字符是否是字母（A-Z 或 a-z）
|
|||
|
|
if ('A' <= char <= 'Z') or ('a' <= char <= 'z'):
|
|||
|
|
letters_str += char
|
|||
|
|
return letters_str
|
|||
|
|
|
|||
|
|
def extract_innermost_bracket_content(self, input_string):
|
|||
|
|
"""
|
|||
|
|
提取给定字符串中最内层尖括号<>内的内容。
|
|||
|
|
|
|||
|
|
参数:
|
|||
|
|
input_string (str): 输入的字符串。
|
|||
|
|
|
|||
|
|
返回:
|
|||
|
|
str: 最内层尖括号内的内容，如果没有匹配则返回空字符串。
|
|||
|
|
"""
|
|||
|
|
depth = 0 # 嵌套深度计数器
|
|||
|
|
max_depth = 0 # 最大深度
|
|||
|
|
content_at_depth = {} # 存储每个深度的内容
|
|||
|
|
for char in input_string:
|
|||
|
|
if char == '<':
|
|||
|
|
depth += 1
|
|||
|
|
max_depth = max(max_depth, depth) # 更新最大深度
|
|||
|
|
content_at_depth[depth] = "" # 为新的深度初始化内容
|
|||
|
|
elif char == '>':
|
|||
|
|
if depth > 0: # 只有在有效的深度下才减少深度
|
|||
|
|
depth -= 1
|
|||
|
|
elif depth > 0: # 只有在尖括号内部时才添加内容
|
|||
|
|
content_at_depth[depth] += char
|
|||
|
|
|
|||
|
|
# 从记录的最大深度获取内容
|
|||
|
|
return content_at_depth.get(max_depth, "")
|
|||
|
|
|
|||
|
|
# 提取字符和数字
|
|||
|
|
def extract_numbers_and_letters_combined(self, text):
|
|||
|
|
combined_str = ''
|
|||
|
|
for char in text:
|
|||
|
|
# 检查字符是否是数字（0-9）或字母（A-Z 或 a-z）
|
|||
|
|
if ('0' <= char <= '9') or ('A' <= char <= 'Z') or ('a' <= char <= 'z'):
|
|||
|
|
combined_str += char
|
|||
|
|
return combined_str
|
|||
|
|
|
|||
|
|
def find_longest_common_substring(self, s1, s2):
|
|||
|
|
# 初始化动态规划表
|
|||
|
|
m, n = len(s1), len(s2)
|
|||
|
|
dp = [[0] * (n + 1) for _ in range(m + 1)]
|
|||
|
|
longest, x_longest = 0, 0
|
|||
|
|
for i in range(1, m + 1):
|
|||
|
|
for j in range(1, n + 1):
|
|||
|
|
if s1[i - 1] == s2[j - 1]:
|
|||
|
|
dp[i][j] = dp[i - 1][j - 1] + 1
|
|||
|
|
if dp[i][j] > longest:
|
|||
|
|
longest = dp[i][j]
|
|||
|
|
x_longest = i
|
|||
|
|
else:
|
|||
|
|
dp[i][j] = 0
|
|||
|
|
# 返回最长公共子字符串
|
|||
|
|
return s1[x_longest - longest: x_longest]
|
|||
|
|
|
|||
|
|
def if_formula(self, text):
|
|||
|
|
# 允许的字符：数字、运算符和括号
|
|||
|
|
allowed_chars = "0123456789+-*/%^(). abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ"
|
|||
|
|
|
|||
|
|
# 检查字符是否合法
|
|||
|
|
for char in text:
|
|||
|
|
if char not in allowed_chars:
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
# 括号匹配检查
|
|||
|
|
bracket_count = 0
|
|||
|
|
for char in text:
|
|||
|
|
if char == '(':
|
|||
|
|
bracket_count += 1
|
|||
|
|
elif char == ')':
|
|||
|
|
bracket_count -= 1
|
|||
|
|
if bracket_count < 0: # 右括号多于左括号
|
|||
|
|
return False
|
|||
|
|
if bracket_count != 0: # 左括号和右括号数量不匹配
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
return True
|
|||
|
|
|
|||
|
|
# 移除字符串内的中文
|
|||
|
|
def remove_chinese_chars(self, text):
|
|||
|
|
"""移除字符串中的中文字符，并返回新的字符串"""
|
|||
|
|
# 定义中文字符的Unicode范围
|
|||
|
|
chinese_char_ranges = [
|
|||
|
|
('\u4e00', '\u9fff'), # 基本汉字
|
|||
|
|
('\u3400', '\u4dbf'), # 扩展A
|
|||
|
|
('\u20000', '\u2a6df'), # 扩展B
|
|||
|
|
# 可以根据需要添加更多的中文范围
|
|||
|
|
]
|
|||
|
|
# 移除中文字符
|
|||
|
|
return ''.join(char for char in text if not any(start <= char <= end for start, end in chinese_char_ranges))
|
|||
|
|
|
|||
|
|
# 循迹
|
|||
|
|
def tracking(self, img):
|
|||
|
|
print("循迹")
|
|||
|
|
roi_blobs_result = self.find_blobs_in_rois(img)
|
|||
|
|
down_center, state_crossing, deflection_angle = self.state_deflection_angle(roi_blobs_result)
|
|||
|
|
|
|||
|
|
dsd = self.data_format_wrapper(down_center, state_crossing, deflection_angle)
|
|||
|
|
self.UsartSend(dsd)
|
|||
|
|
print("下发指令：", dsd)
|
|||
|
|
|
|||
|
|
# 寻找色块 在ROIS中寻找色块，获取ROI中色块的中心区域与是否有色块的信息
|
|||
|
|
def find_blobs_in_rois(self, img):
|
|||
|
|
canvas = img.copy()
|
|||
|
|
roi_blobs_result = {} # 在各个ROI中寻找色块的结果记录
|
|||
|
|
for roi_direct in self.ROIS.keys():
|
|||
|
|
roi_blobs_result[roi_direct] = {
|
|||
|
|
'cx': 0,
|
|||
|
|
'cy': 0,
|
|||
|
|
'w': 0,
|
|||
|
|
'blob_flag': False
|
|||
|
|
}
|
|||
|
|
for roi_direct, roi in self.ROIS.items():
|
|||
|
|
blobs = canvas.find_blobs(self.LINE_COLOR_THRESHOLD, roi=roi, merge=True)
|
|||
|
|
if len(blobs) != 0:
|
|||
|
|
largest_blob = max(blobs, key=lambda b: b.pixels())
|
|||
|
|
if largest_blob.area() > 1000:
|
|||
|
|
roi_blobs_result[roi_direct]['cx'] = largest_blob.cy()
|
|||
|
|
roi_blobs_result[roi_direct]['cy'] = largest_blob.cx()
|
|||
|
|
roi_blobs_result[roi_direct]['w'] = largest_blob.h()
|
|||
|
|
roi_blobs_result[roi_direct]['blob_flag'] = True
|
|||
|
|
x, y, width, height = largest_blob[:4]
|
|||
|
|
img.draw_rectangle((x, y, width, height), color=(255))
|
|||
|
|
else:
|
|||
|
|
# blobs=canvas.find_blobs(LINE_COLOR_THRESHOLD, roi=roi, merge=True, pixels_area=10)
|
|||
|
|
continue
|
|||
|
|
blobs_baise = canvas.find_blobs(self.LINE_COLOR_BAISE, roi=(0, 0, 60, 240),
|
|||
|
|
merge=True) # 车载摄像头屏幕下部找白色#宽度200修改为240#用途寻卡，y示例中40修改为0
|
|||
|
|
blobs_dixing = canvas.find_blobs(self.LINE_COLOR_BAISE, roi=(125, 0, 60, 240),
|
|||
|
|
merge=True) # 车载摄像头屏幕中间找白色 #宽度200修改为240，y示例中40修改为0
|
|||
|
|
blobs_zuo = canvas.find_blobs(self.LINE_COLOR_THRESHOLD, roi=(0, 0, 180, 50), merge=True) # 车载摄像头屏幕左下部找黑色
|
|||
|
|
blobs_you = canvas.find_blobs(self.LINE_COLOR_THRESHOLD, roi=(0, 190, 180, 50), merge=True) # 车载摄像头屏幕右下部找黑色
|
|||
|
|
if len(blobs_baise) != 0:
|
|||
|
|
print("*********进入循环第1步*******")
|
|||
|
|
largest_baise = max(blobs_baise, key=lambda b: b.pixels())
|
|||
|
|
wx, wy, wwidth, wwheight = largest_baise[:4]
|
|||
|
|
arc = wwidth * wwheight
|
|||
|
|
if arc >= 11000:
|
|||
|
|
print("*********进入循环第2步*******")
|
|||
|
|
if len(blobs_dixing) != 0:
|
|||
|
|
print("*********进入循环第3步*******")
|
|||
|
|
largest_dixing = max(blobs_dixing, key=lambda b: b.pixels())
|
|||
|
|
wx, wy, wwidth, wwheight = largest_dixing[:4]
|
|||
|
|
arc = wwidth * wwheight
|
|||
|
|
if arc >= 11000:
|
|||
|
|
print('kapian') # 首先中部区域识别到白色进入判断地形还是卡片，接着判断下部，如果为白色判断为卡片。
|
|||
|
|
self.UsartSend(self.data_format_wrapper(0, 1, 0)) # 地形停止命令
|
|||
|
|
if len(blobs_zuo) != 0 and len(blobs_you) != 0:
|
|||
|
|
print("ka十字路口")
|
|||
|
|
self.UsartSend(self.data_format_wrapper(1, 1, 0)) # 地形停止命令
|
|||
|
|
else:
|
|||
|
|
print('dixing')
|
|||
|
|
print(roi_blobs_result) # 返回的是黑色色块，各区域中心位置
|
|||
|
|
self.UsartSend(self.data_format_wrapper(0, 1, 0)) # 地形停止命令
|
|||
|
|
return roi_blobs_result # 返回的是黑色色块，各区域中心位置
|
|||
|
|
|
|||
|
|
# 计算偏转状态值
|
|||
|
|
def state_deflection_angle(self, roi_blobs_result):
|
|||
|
|
'''
|
|||
|
|
说明：偏转状态值返回
|
|||
|
|
'''
|
|||
|
|
# ROI区域权重值
|
|||
|
|
# ROIS_WEIGHT = [1, 1, 1, 1]
|
|||
|
|
ROIS_WEIGHT = [1, 0, 0, 1]
|
|||
|
|
state_crossing = False
|
|||
|
|
deflection_angle = 0 # 偏转角
|
|||
|
|
down_center = 0 # 中下值
|
|||
|
|
center_num = 0 # 中间值
|
|||
|
|
# print(roi_blobs_result)
|
|||
|
|
|
|||
|
|
ROIS_WEIGHT_num=0
|
|||
|
|
# 偏转值计算，ROI中心区域X值
|
|||
|
|
centroid_sum = roi_blobs_result['up']['cx'] * ROIS_WEIGHT[0] + roi_blobs_result['middle_up']['cx'] * ROIS_WEIGHT[1] \
|
|||
|
|
+ roi_blobs_result['middle_down']['cx'] * ROIS_WEIGHT[2] + roi_blobs_result['down']['cx'] * ROIS_WEIGHT[3]
|
|||
|
|
if roi_blobs_result['up']['blob_flag']:
|
|||
|
|
center_num += roi_blobs_result['up']['cx'] *ROIS_WEIGHT[0]
|
|||
|
|
ROIS_WEIGHT_num+=ROIS_WEIGHT[0]
|
|||
|
|
|
|||
|
|
if roi_blobs_result['middle_up']['blob_flag']:
|
|||
|
|
center_num += roi_blobs_result['middle_up']['cx'] *ROIS_WEIGHT[1]
|
|||
|
|
ROIS_WEIGHT_num+=ROIS_WEIGHT[1]
|
|||
|
|
|
|||
|
|
if roi_blobs_result['middle_down']['blob_flag']:
|
|||
|
|
center_num += roi_blobs_result['middle_down']['cx'] *ROIS_WEIGHT[2]
|
|||
|
|
ROIS_WEIGHT_num+=ROIS_WEIGHT[2]
|
|||
|
|
|
|||
|
|
if roi_blobs_result['down']['blob_flag']:
|
|||
|
|
center_num += roi_blobs_result['down']['cx'] *ROIS_WEIGHT[3]
|
|||
|
|
ROIS_WEIGHT_num+=ROIS_WEIGHT[3]
|
|||
|
|
|
|||
|
|
|
|||
|
|
center_pos = center_num / ROIS_WEIGHT_num
|
|||
|
|
deflection_angle = (self.IMG_WIDTH / 2) - center_pos
|
|||
|
|
|
|||
|
|
# 判断两侧ROI区域检测到黑色线
|
|||
|
|
if roi_blobs_result['left']['blob_flag'] or roi_blobs_result['right']['blob_flag']:
|
|||
|
|
# 判断两侧ROI区域检测到黑色线处于图像下方1/3处
|
|||
|
|
if roi_blobs_result['left']['cy'] <= ((self.IMG_HEIGHT / 3)) or roi_blobs_result['right']['cy'] <= (
|
|||
|
|
(self.IMG_HEIGHT / 3)):
|
|||
|
|
# 当最下方ROI区域的黑线宽度大于140像素（检测到路口）
|
|||
|
|
if roi_blobs_result['down']['w'] > 140:
|
|||
|
|
down_center = 1 # 自行修改处 判断识别到十字路口
|
|||
|
|
print("输出了十字路口标识")
|
|||
|
|
|
|||
|
|
return down_center, state_crossing, deflection_angle
|
|||
|
|
|
|||
|
|
# 控制舵机
|
|||
|
|
def Tise_servo(self, angle):
|
|||
|
|
# 判断舵机控制方向
|
|||
|
|
if angle < 0:
|
|||
|
|
# 限制舵机角度，防止过大损坏舵机
|
|||
|
|
if angle > 80:
|
|||
|
|
angle = 80
|
|||
|
|
angle = -angle
|
|||
|
|
elif angle > 0:
|
|||
|
|
# 限制舵机角度，防止过大损坏舵机
|
|||
|
|
if angle > 35:
|
|||
|
|
angle = 35
|
|||
|
|
angle = angle
|
|||
|
|
# PWM通过定时器配置，接到IO17引脚
|
|||
|
|
tim_pwm = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
|
|||
|
|
S1 = PWM(tim_pwm, freq=50, duty=0, pin=17)
|
|||
|
|
S1.duty((angle + 90) / 180 * 10 + 2.5)
|
|||
|
|
|
|||
|
|
def Servo(self, data):
|
|||
|
|
'''
|
|||
|
|
功能：180度舵机：angle:-90至90 表示相应的角度
|
|||
|
|
360连续旋转度舵机：angle:-90至90 旋转方向和速度值。
|
|||
|
|
【duty】占空比值：0-100
|
|||
|
|
'''
|
|||
|
|
angle = data[5]
|
|||
|
|
# 判断舵机控制方向
|
|||
|
|
if data[4] == ord('-'):
|
|||
|
|
# 限制舵机角度，防止过大损坏舵机
|
|||
|
|
if angle > 80:
|
|||
|
|
angle = 80
|
|||
|
|
angle = -angle
|
|||
|
|
elif data[4] == ord('+'):
|
|||
|
|
# 限制舵机角度，防止过大损坏舵机
|
|||
|
|
if angle > 35:
|
|||
|
|
angle = 35
|
|||
|
|
angle = angle
|
|||
|
|
# PWM通过定时器配置，接到IO17引脚
|
|||
|
|
tim_pwm = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
|
|||
|
|
S1 = PWM(tim_pwm, freq=50, duty=0, pin=17)
|
|||
|
|
S1.duty((angle + 90) / 180 * 10 + 2.5)
|
|||
|
|
|
|||
|
|
# 检验校验和
|
|||
|
|
def verify_checksum(self, data):
|
|||
|
|
if len(data) != 8:
|
|||
|
|
return False
|
|||
|
|
# 计算校验和：data[2]、data[3]、data[4] 和 data[5] 的和，然后对256取模
|
|||
|
|
calculated_checksum = (data[2] + data[3] + data[4] + data[5]) % 256
|
|||
|
|
# 比较计算出的校验和与data[6]是否相等
|
|||
|
|
if calculated_checksum == data[6]:
|
|||
|
|
return True
|
|||
|
|
else:
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
# 串口发送
|
|||
|
|
def UsartSend(self, str_data):
|
|||
|
|
'''
|
|||
|
|
串口发送函数
|
|||
|
|
'''
|
|||
|
|
print(str_data)
|
|||
|
|
self.uart.write(str_data)
|
|||
|
|
|
|||
|
|
# 判断符号
|
|||
|
|
def get_symbol(self, num):
|
|||
|
|
'''
|
|||
|
|
根据数值正负，返回数值对应的符号
|
|||
|
|
正数： ‘+’， 负数‘-’ 主要为了方便C语言解析待符号的数值。
|
|||
|
|
'''
|
|||
|
|
print("num = ", num)
|
|||
|
|
if num >= 0:
|
|||
|
|
return ord('+')
|
|||
|
|
else:
|
|||
|
|
return ord('-')
|
|||
|
|
|
|||
|
|
# 封装数据
|
|||
|
|
def data_format_wrapper(self, down_center, state_crossing, deflection_angle):
|
|||
|
|
'''
|
|||
|
|
根据通信协议封装循迹数据
|
|||
|
|
TODO 重新编写通信协议 与配套C解析代码
|
|||
|
|
'''
|
|||
|
|
send_data = [
|
|||
|
|
0x55,
|
|||
|
|
0x02,
|
|||
|
|
0x91,
|
|||
|
|
down_center, # 底部色块中心是否在中点附近#底部色块十字路口
|
|||
|
|
1 if state_crossing else 0, # 是否越障 无用
|
|||
|
|
self.get_symbol(deflection_angle), # 偏航角符号 print输出是43 + 45- +向左转调整 -向右转调整
|
|||
|
|
abs(int(deflection_angle)), # 偏航角
|
|||
|
|
0xbb]
|
|||
|
|
return bytes(send_data)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 运行程序
|
|||
|
|
myMain = Mainlen()
|
|||
|
|
myMain.startMain()
|