xdl/Embedded_game
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
8f82b189a1
Embedded_game/002_B_Car/track_7.10.py

594 lines
23 KiB
Python
Raw Normal View History

1
2025-01-02 12:48:11 +08:00
'''
K210 视频循迹示例
'''
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
# Debug模式开关，打开后方便显示调试信息
#is_debug = True
is_debug = False
QR_num = 0
JTD = False
QR_Flag = False
Flag_track = False
#--------------感光芯片配置 START -------------------
DISTORTION_FACTOR = 1 # 设定畸变系数
IMG_WIDTH = 240
IMG_HEIGHT = 320
def init_sensor():
'''
初始化感光芯片
'''
lcd.init(freq=15000000) #初始化LCD
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_auto_gain(0,0)
#sensor.set_auto_exposure(1) # 设置自动曝光
sensor.set_auto_gain(0, gain_db = 8) # 设置画面增益 17 dB 影响实时画面亮度
#sensor.set_auto_whitebal(0, rgb_gain_db = (0,0,0))
sensor.skip_frames(time = 2000) # 等待设置生效
clock = time.clock() # 创建一个时钟来追踪 FPS（每秒拍摄帧数）
init_sensor()
#--------------感光芯片配置 END -------------------
#--------------串口UART部分 START -------------------
#映射串口引脚
fm.register(6, fm.fpioa.UART1_RX, force=True)
fm.register(7, fm.fpioa.UART1_TX, force=True)
#初始化串口
uart = UART(UART.UART1, 115200, read_buf_len=4096)
def get_symbol(num):
'''
根据数值正负，返回数值对应的符号
正数： ‘+’， 负数‘-’ 主要为了方便C语言解析待符号的数值。
尝试，若为0，输出‘,’。
'''
if num > 0:
return ord('+')
elif num < 0:
return ord('-')
else :
return ord(',')
def data_format_wrapper(down_center, state_crossing, deflection_angle):
'''
根据通信协议封装循迹数据
TODO 重新编写通信协议 与配套C解析代码
'''
send_data = [
0x55,
0x02,
0x91,
down_center, # 底部色块中心是否在中点附近#底部色块十字路口
1 if state_crossing else 0, #是否越障
get_symbol(deflection_angle), # 偏航角符号 print输出是43 + 45- +向左转调整 -向右转调整 44 , 不调整
abs(int(deflection_angle)), # 偏航角
0xbb
]
global is_debug
if is_debug:
print(send_data)
return bytes(send_data)
def UsartSend(str_data):
'''
串口发送函数
'''
uart.write(str_data)
#--------------串口UART部分 END -------------------
#--------------定时器部分 START -------------------
is_need_send_data = False # 是否需要发送数据的信号标志
def uart_time_trigger(tim):
'''
串口发送数据的定时器，定时器的回调函数
'''
global is_need_send_data, QR_Flag, QR_num
if QR_Flag:
QR_num += 1
if QR_num >= 200: # 10秒计时
QR_Flag = False
QR_num = 0
is_need_send_data = True
'''
初始化定时器, 每秒执行20次
period ： 周期1000/20=50
callback： 回调函数
'''
tim1 = Timer(Timer.TIMER1, Timer.CHANNEL1, mode=Timer.MODE_PERIODIC, period=50, callback=uart_time_trigger)
#--------------定时器部分 END -------------------
#--------------舵机配置 START -------------------
def Servo(angle):
'''
说明：舵机控制函数
功能：180度舵机：angle:-90至90 表示相应的角度
360连续旋转度舵机：angle:-90至90 旋转方向和速度值。
【duty】占空比值：0-100
'''
#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)
Servo(0) # 默认向下0度
time.sleep(1)
#--------------舵机配置 END -------------------
#--------------直线检测部分 START -------------------
# 直线灰度图颜色阈值
LINE_COLOR_THRESHOLD = [(0, 60)] #找黑色
LINE_COLOR_BAISE=[(60,255)] #找白色
#LIGHT_LINE_COLOR_THRESHOLD = [(9, 88)]
#LINE_COLOR_THRESHOLD = [(0, 52, -19, 0, -1, 14)]
# 以实际空间坐标为基准，取样窗口
'''
********屏幕ROI区域*******
* | ------上方----- |
* 左 ------中上----- 右
* 侧 ------中下----- 侧
* | ------下方----- |
********屏幕ROI区域*******
'''
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), # 横向取样-下方
}
#找黑色
ROIS_BAISE = {
'wleft':(110, 0, 100, 20),
'wright':(110, 220, 100, 20),
'wconte':(110, 45, 100, 150), #白色中心区域
}
#找白色
def find_blobs_in_rois(img):
'''
说明：在ROIS中寻找色块，获取ROI中色块的中心区域与是否有色块的信息
'''
global ROIS # global定义全局变量 字典数组
global is_debug # Debug模式开关，打开后方便显示调试信息
canvas = img.copy() # 复制给 canvas 实时传输的图片 img类型
canvas_baise = img.copy() # 判断白色的图片
roi_blobs_result = {} # 在各个ROI中寻找色块的结果记录 空的 定义的
roi_blobs_baise = {} # 定义白色的色块寻找色块的结果记录 空的 定义的
for roi_direct in ROIS.keys(): # 赋值初始化键值 黑线
roi_blobs_result[roi_direct] = {
'cx':0,
'cy':0,
'w':0,
'blob_flag': False # 标志位
}
for roi_baise in ROIS_BAISE.keys(): # 赋值初始化键值 白色
roi_blobs_result[roi_baise] = {
'wcx':0,
'wcy':0,
'ww':0,
'wblob_baise': False # 标志位
}
for roi_direct, roi in ROIS.items(): # 返回键值对 赋值给前两个参数 第一个是键 第二个是值
blobs=canvas.find_blobs(LINE_COLOR_THRESHOLD, roi=roi,area_threshold=400,merge=True)
# 查找图像中所有区域黑色色块，并返回一个包括每个色块的色块对象的列表
# 判断返回列表不为空
if len(blobs) != 0:
largest_blob = max(blobs, key=lambda b: b.pixels())
if largest_blob.area() > 1000: # 判断色块返回面积是否大于1000
x,y,width,height = largest_blob[:4] # 将列表0到3
#roi_blobs_result[roi_direct]['cx'] = largest_blob.cy()-40 #存疑为什么-40 #此处XY轴互换
roi_blobs_result[roi_direct]['cx'] = largest_blob.cy() #存疑为什么-40 #此处XY轴互换，返回色块的外框的中心Y坐标
roi_blobs_result[roi_direct]['cy'] = largest_blob.cx() #存疑为什么-40 #此处XY轴互换，返回色块的外框的中心X坐标
roi_blobs_result[roi_direct]['w'] = largest_blob.h()
roi_blobs_result[roi_direct]['blob_flag'] = True
if is_debug:
print('LINE_COLOR_THRESHOLD*****')
#print(largest_blob[:4] )
#print('largest_blob内容*****')
#print(largest_blob)
#koimg.draw_rectangle((x,y,width, height), color=(0))
#img.draw_rectangle((0, 0, 180, 50), color=(255))
else:
#blobs=canvas.find_blobs(LINE_COLOR_THRESHOLD, roi=roi, merge=True, pixels_area=10)
continue
blobs_baise = canvas.find_blobs(LINE_COLOR_BAISE, roi=(0, 0, 60, 240), merge=True)#车载摄像头屏幕下部找白色#宽度200修改为240#用途寻卡，y示例中40修改为0
blobs_dixing = canvas.find_blobs(LINE_COLOR_BAISE, roi=(125, 0, 60, 240), merge=True)#车载摄像头屏幕中间找白色 #宽度200修改为240，y示例中40修改为0
blobs_zuo = canvas.find_blobs(LINE_COLOR_THRESHOLD, roi=(0, 0, 180, 50), merge=True)#车载摄像头屏幕左下部找黑色
blobs_you = canvas.find_blobs(LINE_COLOR_THRESHOLD, roi=(0, 190, 180, 50), merge=True)#车载摄像头屏幕右下部找黑色
#if len(blobs_baise) != 0:#进行了修改先让摄像头中部区域先碰到白色
if len(blobs_baise) != 0:
print("*********进入循环第1步*******")
largest_baise = max(blobs_baise , key = lambda b:b.pixels())
wx,wy,wwidth,wwheight = largest_baise[:4]
#print("宽度",largest_baise[2])
#print("高度",largest_baise[3])
#largest_blobs_zuo = max(blobs_zuo , key = lambda b:b.pixels())
#Bx_L,By_L,Bwidth_L,Bwheight_L = largest_blobs_zuo[:4]
#arc_L=Bwidth_L*Bwheight_L
#largest_blobs_you = max(blobs_you , key = lambda b:b.pixels())
#Bx_R,By_R,Bwidth_R,Bwheight_R = largest_blobs_you[:4]
#arc_R=Bwidth_R*Bwheight_R
if is_debug:
#print('blobs_baise*****')
img.draw_rectangle((0, 0, 80, 240), color=(255))
#img.draw_rectangle((0, 190, 180, 50), color=(255))
#state_crossing = True #线上为白色准备跨越
#img.draw_rectangle((wx,wy,wwidth, wwheight), color=(255)) #标注
arc = wwidth * wwheight
if arc >= 11000:
print("*********进入循环第2步*******")
#UsartSend(data_format_wrapper(0, 1, 0))
#img.draw_rectangle((wx,wy,wwidth, wwheight), color=(255))#左侧区域
if len(blobs_dixing) != 0:
print("*********进入循环第3步*******")
largest_dixing = max(blobs_dixing , key = lambda b:b.pixels())
wx,wy,wwidth,wwheight = largest_dixing[:4]
#state_crossing = True
#img.draw_rectangle((wx,wy,wwidth, wwheight), color=(255))
arc = wwidth * wwheight
if arc >= 11000:
#UsartSend(data_format_wrapper(0, 1, 0))
#img.draw_rectangle((wx,wy,wwidth, wwheight), color=(255))#右侧区域
print('kapian') #首先中部区域识别到白色进入判断地形还是卡片，接着判断下部，如果为白色判断为卡片。
UsartSend(data_format_wrapper(0, 1, 0))#地形停止命令
if len(blobs_zuo) != 0 and len(blobs_you) != 0:
print("ka十字路口")
UsartSend(data_format_wrapper(1, 1, 0))#地形停止命令
else:
print('dixing')
print(roi_blobs_result) #返回的是黑色色块，各区域中心位置
UsartSend(data_format_wrapper(0, 1, 0))#地形停止命令
return roi_blobs_result #返回的是黑色色块，各区域中心位置
def state_deflection_angle(roi_blobs_result):
'''
说明：偏转状态值返回
'''
# ROI区域权重值
#ROIS_WEIGHT = [0, 1, 0, 1]
ROIS_WEIGHT = [1, 0, 0, 1] # ROIS对白色权重的数组
state_crossing = False
deflection_angle = 0 # 偏转角
down_center = 0 # 中下值
center_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 += ROIS_WEIGHT[0]
if roi_blobs_result['middle_up']['blob_flag']:
center_num += ROIS_WEIGHT[1]
if roi_blobs_result['middle_down']['blob_flag']:
center_num += ROIS_WEIGHT[2]
if roi_blobs_result['down']['blob_flag']:
center_num += ROIS_WEIGHT[3]
center_pos = centroid_sum / (ROIS_WEIGHT[0]+ROIS_WEIGHT[1]+ROIS_WEIGHT[2]+ROIS_WEIGHT[3])
deflection_angle = (IMG_WIDTH/2)- center_pos
#判断两侧ROI区域检测到黑色线
if roi_blobs_result['left']['blob_flag'] and roi_blobs_result['right']['blob_flag']:
# 判断两侧ROI区域检测到黑色线处于图像下方1/3处
if roi_blobs_result['left']['cy'] <= ((IMG_HEIGHT/3)) or roi_blobs_result['right']['cy'] <= ((IMG_HEIGHT/3)):
# 当最下方ROI区域的黑线宽度大于140像素（检测到路口）
if roi_blobs_result['down']['w'] > 140:
down_center=1#自行修改处 判断识别到十字路口
print("输出了十字路口标识")
if roi_blobs_result['left']['blob_flag'] and roi_blobs_result['right']['blob_flag']:
# 判断两侧ROI区域检测到黑色线处于图像下方1/3处
if roi_blobs_result['left']['cy'] <= ((IMG_HEIGHT/3)) or roi_blobs_result['right']['cy'] <= ((IMG_HEIGHT/3)):
# 当最下方ROI区域的黑线宽度大于140像素（检测到路口）
if roi_blobs_result['down']['w'] > 140:
0
return down_center, state_crossing, deflection_angle
#--------------直线与路口检测部分 END -------------------
#--------------二维码识别部分 START -------------------
def QR_Check(img):
'''
说明：二维码函数
'''
hld = 0x01
QR_rioRed =(28, 43, 3, 29, 6, 15)
QR_rioGreen = (23, 37, -21, -13, 7, 20)
QR_rioBlue = (8, 31, 3, 15, -36, -22)
res = img.find_qrcodes() # 寻找二维码
if len(res) > 0:
# 在图片和终端显示二维码信息
#img.draw_rectangle(res[0].rect())
red_QR = img.find_blobs([QR_rioRed], rio=res[0].rect(), area_threshold=5000, merge = True)
green_QR = img.find_blobs([QR_rioGreen], rio=res[0].rect(), area_threshold=5000, merge = True)
blue_QR = img.find_blobs([QR_rioBlue], rio=res[0].rect(), area_threshold=5000, merge = True)
#img.draw_string(2,2, res[0].payload(), color=(0,128,0), scale=2)
if ((len(red_QR) != 0) and (120 <= (res[0].x() + res[0].w()/2) <=200) and (60 <= (res[0].y() + res[0].h()/2) <=180)):
x1 = red_QR[0][0]
y1 = red_QR[0][1]
w1 = red_QR[0][2]
h1 = red_QR[0][3]
img.draw_rectangle(res[0].rect())
print(res[0].payload())
print("红色")
hld = 0x01
elif ((len(green_QR) != 0) and (120 <= (res[0].x() + res[0].w()/2) <=200) and (60 <= (res[0].y() + res[0].h()/2) <=180)):
x1 = green_QR[0][0]
y1 = green_QR[0][1]
w1 = green_QR[0][2]
h1 = green_QR[0][3]
img.draw_rectangle(res[0].rect())
print(res[0].payload())
print("绿色")
hld = 0x02
elif ((len(blue_QR) != 0) and (120 <= (res[0].x() + res[0].w()/2) <=200) and (60 <= (res[0].y() + res[0].h()/2) <=180)):
x1 = blue_QR[0][0]
y1 = blue_QR[0][1]
w1 = blue_QR[0][2]
h1 = blue_QR[0][3]
img.draw_rectangle(res[0].rect())
print(res[0].payload())
print("蓝色")
hld = 0x03
elif ((120 <= (res[0].x() + res[0].w()/2) <=200) and (60 <= (res[0].y() + res[0].h()/2) <=180)):
img.draw_rectangle(res[0].rect())
print(res[0].payload())
print("黑色")
hld = 0x04
#串口发送二维码信息
uart.write(bytes([0x55]))
uart.write(bytes([0x92]))
uart.write(bytes([0x01]))
uart.write(bytes([hld]))
uart.write(bytes([len(res[0].payload())]))
for qrdata in res[0].payload():
uart.write(bytes([ord(qrdata)]))
uart.write(bytes([0xbb]))
#--------------二维码识别部分 END -------------------
#-----------------交通灯识别 START-------------------#
###################################################################################################
# 返回值
# 1、红灯（蒙的答案选项）
# 2、绿灯
# 3、黄灯
#
# 需要完善部分：
# 1、过滤掉范围过小和过大部分--》提高识别效率
# 2、通信协议
###################################################################################################
def tick(timer):
global times,isCut
#print(times)
if(times == 4):
isCut = False
tim.deinit() #tim对象反初始化
times = 0
else:
times+=1;
def drawTrafficLight(isDraw): #识别红绿灯*******************
red = (49, 68, 40, 65, 23, 69) #红色阈值
green = (66, 93, -54, -32, 36, 52) #绿色阈值
yellow = (62, 98, -17, 22, 41, 68) #黄色阈值
hld = 0x01 #蒙答案
try:
TrafficLight_rio = (240, 0, 80, 240)
img.draw_rectangle(TrafficLight_rio, color=(255))
#tim.callback(tick) #回调函数的形参是用户函数或者匿名函数都必须有一个位置参数,才可以正常工作（位置参数可能就是一个实例），而且调用时不用些括号
red_blobs = img.find_blobs([red], rio=TrafficLight_rio,merge=True) #寻找红色色块 合并所有重叠的blod
green_blobs = img.find_blobs([green], rio=TrafficLight_rio,merge=True)
yellow_blobs = img.find_blobs([yellow],rio=TrafficLight_rio,merge=True)
if (len(red_blobs) != 0):
x1 = red_blobs[0][0]
y1 = red_blobs[0][1]
w1 = red_blobs[0][2]
h1 = red_blobs[0][3]
print("红灯")
if isDraw is True:
img.draw_rectangle((x1,y1,w1,h1),color=(255,0,0))
hld = 0x01
elif (len(green_blobs) != 0):
x1 = green_blobs[0][0]
y1 = green_blobs[0][1]
w1 = green_blobs[0][2]
h1 = green_blobs[0][3]
print("绿灯")
if isDraw is True:
img.draw_rectangle((x1,y1,w1,h1),color=(0,255,0))
hld = 0x02
elif (len(yellow_blobs) != 0):
x1 = yellow_blobs[0][0]
y1 = yellow_blobs[0][1]
w1 = yellow_blobs[0][2]
h1 = yellow_blobs[0][3]
print("黄灯")
if isDraw is True:
img.draw_rectangle((x1,y1,w1,h1),color=(255,255,255))
hld = 0x03
# 串口发送交通灯信息
uart.write(bytes([0x55]))
uart.write(bytes([0x92]))
uart.write(bytes([0x03]))
uart.write(bytes([hld]))
uart.write(bytes([0x03]))
uart.write(bytes([0x03]))
uart.write(bytes([0x03]))
uart.write(bytes([0xbb]))
except Exception as e:
print("识别红绿灯异常",e)
# 串口发送交通灯信息
uart.write(bytes([0x55]))
uart.write(bytes([0x92]))
uart.write(bytes([0x03]))
uart.write(bytes([hld]))
uart.write(bytes([0x03]))
uart.write(bytes([0x03]))
uart.write(bytes([0x03]))
uart.write(bytes([0xbb]))
#======================交通灯识别 END===========================#
#---------------------MAIN-----------------------
last_cx = 0
last_cy = 0
#将蓝灯引脚IO12配置到GPIO0，K210引脚支持任意配置
fm.register(12, fm.fpioa.GPIO0)
LED_B = GPIO(GPIO.GPIO0, GPIO.OUT) #构建LED对象
#按键KEY用于清屏
fm.register(16, fm.fpioa.GPIO1, force=True)
btn_debug = GPIO(GPIO.GPIO1, GPIO.IN)
x_num = 0
while True:
LED_B.value(0) #点亮LED
# 串口数据接收处理
data = uart.read(8)
if data is not None:
print(data)
print(len(data))
print(binascii.hexlify(data).decode('utf_8'))
if(len(data) >= 8):
if((data[1] == 0x02)&(data[7] == 0xBB)):
# 巡线与控制舵机
if(data[2] == 0x91):
if(data[3] == 0x01): # 启动识别
Servo(-75)
sensor.set_pixformat(sensor.GRAYSCALE) # 设置像素格式为灰色
print("巡线")
print("开始识别")
Flag_track = True
tim1.start()
if(data[3] == 0x02):
print("巡线")
print("停止识别")
Flag_track = False
tim1.stop() # 停止定时器
if(data[3] == 0x03): # 舵机调整
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
Servo(angle) # 控制舵机
# 二维码识别
if(data[2] == 0x92):
print("识别二维码")
sensor.set_pixformat(sensor.RGB565) # 设置像素格式为彩色 RGB565
if(data[3] == 0x01): #启动识别
print("开始识别")
tim1.start()
QR_Flag = True
if(data[3] == 0x02):
print("停止识别")
tim1.stop() # 停止定时器
QR_Flag = False
if(data[3] == 0x03):
print("开始识别")
tim1.start()
JTD = True
if(data[3] == 0x04):
print("停止识别")
tim1.stop()
JTD = False
# 拍摄图片
img = sensor.snapshot()
# 去除图像畸变
img.lens_corr(DISTORTION_FACTOR)
# 二维码识别
#QR_Flag = True
if QR_Flag:
QR_Check(img)
#JTD = 1
if JTD:
#Servo(10)
drawTrafficLight(img)
## 巡线
if is_debug:
Flag_track=1
Servo(-75)
if Flag_track:
roi_blobs_result = find_blobs_in_rois(img)
down_center, state_crossing, deflection_angle = state_deflection_angle(roi_blobs_result)
if is_need_send_data:
UsartSend(data_format_wrapper(down_center, state_crossing, deflection_angle))
print("下发指令")
print(data_format_wrapper(down_center, state_crossing, deflection_angle))
#time.sleep_ms(10)
#is_need_send_data = False
# 在LCD上显示
lcd.display(img)
#按键KEY按下。开启或关闭调试，并退出所有任务。
if btn_debug.value() == 0:
Flag_track = not Flag_track # 巡线任务退出
QR_Flag = not QR_Flag # 二维码任务退出
JTD = not JTD # 交通灯任务退出
is_debug = not is_debug # 调试标志位取
Reference in New Issue Copy Permalink