|
|
|
|
@ -35,11 +35,12 @@ def LCDDrawText(str_data,_image):
|
|
|
|
|
del LCD_Text_Array[0]
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def LCDDrawTrackInfo(down_center,state_crossing,deflection_angle,_image):
|
|
|
|
|
def LCDDrawTrackInfo(down_center,state_crossing,deflection_angle,_image,white_flag):
|
|
|
|
|
lcd.display(_image)
|
|
|
|
|
lcd.draw_string(0, 0, "down_center = " + str(down_center), lcd.GREEN)
|
|
|
|
|
lcd.draw_string(0, 15, "state_crossing = " + str(state_crossing), lcd.GREEN)
|
|
|
|
|
lcd.draw_string(0, 30, "deflection_angle = " + str(deflection_angle), lcd.GREEN)
|
|
|
|
|
lcd.draw_string(0, 45, "white_flag = " + str(white_flag), lcd.GREEN)
|
|
|
|
|
|
|
|
|
|
#--------------LCD 滚动字符显示 STOP------------------
|
|
|
|
|
|
|
|
|
|
@ -110,10 +111,11 @@ def data_format_wrapper(down_center, state_crossing, deflection_angle):
|
|
|
|
|
return bytes(send_data)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def SendWhiteData(is_detect, detect_info=0x00):
|
|
|
|
|
def SendDetectData(is_detect, detect_info=0x00):
|
|
|
|
|
data = [0x55,0x02,0x80,
|
|
|
|
|
0x02 if is_detect else 0x01,
|
|
|
|
|
detect_info if is_detect else 0x00,0x00,0x00,0xBB]
|
|
|
|
|
abs(int(detect_info)) if is_detect else 0x00,
|
|
|
|
|
get_symbol(detect_info),0x00,0xBB]
|
|
|
|
|
UsartSend(bytes(data))
|
|
|
|
|
|
|
|
|
|
def SendROIData(roi_data):
|
|
|
|
|
@ -187,6 +189,7 @@ def Servo(angle):
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Servo(0) # 默认向下0度
|
|
|
|
|
#Servo(-60) # 取样调试，调整至循迹角度
|
|
|
|
|
time.sleep(1)
|
|
|
|
|
|
|
|
|
|
#--------------舵机配置 END -------------------
|
|
|
|
|
@ -198,8 +201,8 @@ time.sleep(1)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 直线灰度图颜色阈值
|
|
|
|
|
LINE_COLOR_THRESHOLD = [(6, 65)] #Dark
|
|
|
|
|
WHITE_BLOCK_THRESHOLD = [(165, 255)]
|
|
|
|
|
LINE_COLOR_THRESHOLD = [(0, 100)] #巡线阈值
|
|
|
|
|
WHITE_BLOCK_THRESHOLD = [(180, 255)] #RFID/特殊地形标志物阈值
|
|
|
|
|
#LINE_COLOR_THRESHOLD = [(6, 120)] #Light
|
|
|
|
|
#LINE_COLOR_THRESHOLD = [(0, 52, -19, 0, -1, 14)]
|
|
|
|
|
|
|
|
|
|
@ -335,20 +338,19 @@ def state_deflection_angle(roi_blobs_result):
|
|
|
|
|
|
|
|
|
|
white_line_flag = 0
|
|
|
|
|
|
|
|
|
|
def rfid_and_spec_terrain_detect(roi_blobs_result):
|
|
|
|
|
def rfid_and_spec_terrain_detect(roi_blobs_result, roi_black_result):
|
|
|
|
|
global white_line_flag
|
|
|
|
|
if roi_blobs_result['up']['w'] > 100 and white_line_flag == 0:
|
|
|
|
|
if roi_black_result['up']['w'] > 100:
|
|
|
|
|
white_line_flag = 1;
|
|
|
|
|
return False
|
|
|
|
|
if roi_blobs_result['middle_up']['w'] > 100 and white_line_flag == 1:
|
|
|
|
|
if roi_blobs_result['middle_up']['w'] > 100:
|
|
|
|
|
white_line_flag = 2;
|
|
|
|
|
return False
|
|
|
|
|
if roi_blobs_result['middle_down']['w'] > 100 and white_line_flag == 2:
|
|
|
|
|
if roi_black_result['down']['w'] > 100:
|
|
|
|
|
white_line_flag = 3;
|
|
|
|
|
return False
|
|
|
|
|
if roi_blobs_result['down']['w'] > 100 and white_line_flag == 3:
|
|
|
|
|
white_line_flag = 0;
|
|
|
|
|
return True
|
|
|
|
|
return False
|
|
|
|
|
#if roi_blobs_result['down']['w'] > 100 and white_line_flag == 3:
|
|
|
|
|
# white_line_flag = 0;
|
|
|
|
|
# return True
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#--------------RFID卡与特殊地形标志物检测 END -------------------
|
|
|
|
|
@ -405,6 +407,13 @@ while True:
|
|
|
|
|
LCDDrawText(binascii.hexlify(data).decode('utf_8'), img)
|
|
|
|
|
if(len(data) >= 8):
|
|
|
|
|
if((data[1] == 0x02)&(data[7] == 0xBB)):
|
|
|
|
|
if(data[2] == 0x80):
|
|
|
|
|
roi_line_check = find_blobs_in_rois(img)
|
|
|
|
|
d_c, s_c, angle = state_deflection_angle(roi_line_check)
|
|
|
|
|
SendDetectData(True, angle)
|
|
|
|
|
#if(roi_line_check['middle_down']['blob_flag'] and roi_line_check['down']['blob_flag']):
|
|
|
|
|
#else:
|
|
|
|
|
#SendDetectData(False)
|
|
|
|
|
# 巡线与控制舵机
|
|
|
|
|
if(data[2] == 0x91):
|
|
|
|
|
if(data[3] == 0x01): # 启动识别
|
|
|
|
|
@ -433,6 +442,10 @@ while True:
|
|
|
|
|
angle = angle
|
|
|
|
|
Servo(angle) # 控制舵机
|
|
|
|
|
time.sleep(1) # 等待舵机角度更新
|
|
|
|
|
if(data[3] == 0xAA):
|
|
|
|
|
roi_line_check = find_blobs_in_rois(img)
|
|
|
|
|
d_c, s_c, angle = state_deflection_angle(roi_line_check)
|
|
|
|
|
SendDetectData(True, angle)
|
|
|
|
|
# 二维码识别
|
|
|
|
|
if(data[2] == 0x92):
|
|
|
|
|
print("识别二维码")
|
|
|
|
|
@ -453,14 +466,14 @@ while True:
|
|
|
|
|
# 巡线
|
|
|
|
|
if Flag_track:
|
|
|
|
|
roi_blobs_result = find_blobs_in_rois(img)
|
|
|
|
|
if rfid_and_spec_terrain_detect(find_white_blobs_in_rois(img)):
|
|
|
|
|
SendWhiteData(False)
|
|
|
|
|
if rfid_and_spec_terrain_detect(find_white_blobs_in_rois(img), roi_blobs_result):
|
|
|
|
|
SendDetectData(False)
|
|
|
|
|
is_need_send_data = False
|
|
|
|
|
else:
|
|
|
|
|
down_center, state_crossing, deflection_angle = state_deflection_angle(roi_blobs_result)
|
|
|
|
|
if state_crossing:
|
|
|
|
|
white_line_flag = 0
|
|
|
|
|
LCDDrawTrackInfo(down_center, state_crossing, deflection_angle, img)
|
|
|
|
|
#if state_crossing:
|
|
|
|
|
# white_line_flag = 0
|
|
|
|
|
LCDDrawTrackInfo(down_center, state_crossing, deflection_angle, img, white_line_flag)
|
|
|
|
|
if is_need_send_data:
|
|
|
|
|
UsartSend(data_format_wrapper(down_center, state_crossing, deflection_angle))
|
|
|
|
|
time.sleep_ms(10)
|
|
|
|
|
|
UnknownObject
2 years ago