#=============================================================
import sensor
import image
import lcd
import time
import machine
from machine import UART, Timer
from fpioa_manager import fm
import ustruct
import sensor, image, lcd, time
import KPU as kpu
import utime
from Maix import GPIO
from board import board_info
import math
# ************************* 一些变量的设定 *************************
min_blob_area = 1 # 最小色块面积阈值
x_offset = 0
y_offset = 0
position = "Center" # 初始化 position
red_x = 160
red_y = 120
green_x = 160
green_y = 120
black_x = 160
black_y = 120
green_threshold = (85, 97, -128, -22, 124, -128) # 设定颜色色块参数
red_threshold = (75, 66, 10, 118, 89, -39)
red_threshold = (72, 100, -50, 127, -128, 127)
black_threshold = (39, 1, -128, 127, -41, 57)
#labels = ['1', '2', '3', '4', '5', '6', '7', '8']
labels = ['0', '1']
color_R = (255, 0, 0)
color_G = (0, 255, 0)
color_B = (0, 0, 255)
color_BLACK = (0,0,0)
color_WHITE = (255,255,255)
# ************************* 摄像头画面的初始化 *************************
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
# 如果对输入的镜头有格式要求
img_width = 240
img_height = 240
sensor_window = (img_width,img_height)
sensor_hmirror = False
sensor_vflip = True
sensor.set_windowing(sensor_window)
sensor.set_hmirror(sensor_hmirror)
sensor.set_vflip(sensor_vflip)
sensor.run(1)
#sensor.skip_frames() #跳过一些图片让摄像头稳定
# ************************* lcd画面的初始化 *************************
lcd.init(type=1) #type=1正常，type=2反色
#lcd.init(freq = 1500000)
lcd.rotation(0) #旋转方向 0-0° 1-90° 2-180° 3-270°
#lcd.display(img) # 显示图像 #在后续完成了位置的标注后，再进行显示
#img.draw_string(50, 50, "img") #在图片上写字
#lcd.draw_string(0, 0, "lcd") #在lcd上写字 #最好在图片上写字，因为这种翻转是有用的
# ************************* 串口的初始化设置 *************************
fm.register(24, fm.fpioa.UART1_TX, force=True)
fm.register(25, fm.fpioa.UART1_RX, force=True)
uart_A = UART(UART.UART1, 9600, 8, 0, 0, timeout=1000, read_buf_len=4096)
uart_A.write(b'ready\r\n')
# ************************* 定时器的中断 *************************
def send_data(timer):
data = "none\r\n".encode()
uart_A.write(data)
'''
if classID != -1:
abs_x_offset_str = str(abs(x_offset))
data = labels[classID].encode() + ":".encode() + position.encode() + abs_x_offset_str.encode() + "\r\n".encode()
else:
data = "none\r\n".encode()
'''
tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PERIODIC, period=500, unit=Timer.UNIT_MS, callback=send_data)
# ************************ 循迹黑色的线 **************************
def track_task():
red_roi_size = 30 # 十字架的尺寸，根据需要调整
# 计算子区域的左上角和右下角坐标
roi_left = max(red_x - red_roi_size, 0) # 左上角x坐标，不能小于0
roi_top = max(red_y - red_roi_size, 0) # 左上角y坐标，不能小于0
roi_right = min(red_x + red_roi_size, img_width) # 右下角x坐标，不能大于图像宽度
roi_bottom = min(red_y + red_roi_size, img_height) # 右下角y坐标，不能大于图像高度
#print("left:",roi_left," top:",roi_top," right:",roi_right," bottom:",roi_bottom)
# 处理边界情况，确保子区域不超出图像范围
red_roi_region = img.copy(roi=(roi_left, roi_top, roi_right, roi_bottom))
blobs = red_roi_region.find_blobs([black_threshold]) # 在img中找色块
#blobs = img.find_blobs([black_threshold]) # 在img中找色块
if blobs:
# 对色块按面积进行排序，从大到小
blobs = sorted(blobs, key=lambda b: b.area(), reverse=True)
#for b in blobs:
for i, b in enumerate(blobs):
if i > 0: # 只处理前三个最大的目标
break
if b.area() > min_blob_area: # 判断色块面积是否大于阈值
# 进行框图表示
itemROL = b.rect()
x, y, w, h = itemROL
print("x:",x," y:",y," w=",w," h=",h)
abs_x = x+roi_left
abs_y = y+roi_top
itemROL = (abs_x, abs_y, w, h)
#print("x:",abs_x," y:",abs_y," w=",w," h=",h)
img.draw_rectangle(itemROL,scale=2, color=color_WHITE) # 画一个矩形框出目标
img.draw_cross(roi_left+b.cx(), roi_top+b.cy(), color=color_WHITE) # 在图像中心画十字叉
img.draw_string(abs_x, abs_y-20,"B", scale=2, color=color_WHITE)
global black_x
global black_y
black_x = abs_x
black_y = abs_y
# ************************* 寻找色块 *************************
while True:
img = sensor.snapshot()
# ----------------------- 黑色方块 ------------------------
#img = img.to_grayscale(copy=False)
# 将图像转换为灰度图像
'''
img_gray = img.to_grayscale(copy=True)
# 找到边缘中的直线段
lines = img_gray.find_lines()
'''
lines = img.find_lines()
# 在图像上绘制直线
i=0
for i,l in lines:
# 只处理较长的直线段
if l.length() > 100:
# 获取直线段的两个端点坐标
x1, y1, x2, y2 = l.line()
# 在图像上画线
img.draw_line((x1, y1, x2, y2), color=color_B, thickness=5)
print("i="i," x1=",x1," y1=",y1," x2=",x2," y2=",y2)
# 计算直线之间的交点
i = 0
for i in range(len(lines)):
for j in range(i + 1, len(lines)):
line1 = lines[i]
line2 = lines[j]
# 获取直线参数
theta1 = line1.theta() # 直线1的角度
rho1 = line1.rho() # 直线1的p值
theta2 = line2.theta() # 直线2的角度
rho2 = line2.rho() # 直线2的p值
# 判断两条直线是否平行
if abs(theta1 - theta2) < 1e-2:
continue
# 计算交点坐标
x = (rho2 * math.sin(math.radians(theta1)) - rho1 * math.sin(math.radians(theta2))) / (math.cos(math.radians(theta1 - theta2))+0.1)
y = (rho1 - x * math.cos(math.radians(theta1))) / (math.sin(math.radians(theta1))+0.1)
# 在图像上画交点
img.draw_circle(int(x), int(y), 5, color=(0, 0, 255), fill=True)
# ----------------------- 红色色块 ------------------------
blobs = img.find_blobs([red_threshold]) # 在img中找色块
if blobs:
# 对色块按面积进行排序，从大到小
blobs = sorted(blobs, key=lambda b: b.area(), reverse=True)
#for b in blobs:
for i, b in enumerate(blobs):
if i > 0: # 只处理前三个最大的目标
break
if b.area() > min_blob_area: # 判断色块面积是否大于阈值
# 进行框图表示
itemROL = b.rect()
x, y, w, h = itemROL
img.draw_rectangle(itemROL,scale=2, color=color_R) # 画一个矩形框出目标
img.draw_cross(b.cx(), b.cy(),color=color_R) # 在图像中心画十字叉
img.draw_string(x, y-20,"R", scale=2, color=color_R)
red_x = b.cx()
red_y = b.cy()
#识别红点周围的黑色线
#track_task()
# ----------------------- 绿色色块 ------------------------
blobs = img.find_blobs([green_threshold]) # 在img中找色块
if blobs:
# 对色块按面积进行排序，从大到小
blobs = sorted(blobs, key=lambda b: b.area(), reverse=True)
#for b in blobs:
i = 0
for i, b in enumerate(blobs):
if i > 0: # 只处理前三个最大的目标
break
if b.area() > min_blob_area: # 判断色块面积是否大于阈值
# 进行框图表示
itemROL = b.rect()
x, y, w, h = itemROL
img.draw_rectangle(itemROL,scale=2, color=color_G) # 画一个矩形框出目标
img.draw_cross(b.cx(), b.cy(),color=color_G) # 在图像中心画十字叉
img.draw_string(x, y-20,"G", scale=2, color=color_G)
green_x = b.cx()
green_y = b.cy()
lcd.display(img) # 先显示图像
lcd.draw_string(10, 10, " RED :" + str(red_x) +','+ str(red_y), lcd.RED, lcd.BLACK) # 再显示文字
lcd.draw_string(10, 30, "BLACK:" + str(black_x) +','+ str(black_y), lcd.WHITE, lcd.BLACK) # 再显示文字
lcd.draw_string(10, 50, "GREEN:" + str(green_x) +','+ str(green_y), lcd.GREEN, lcd.BLACK) # 再显示文字
# ************************* yolo v2 *************************
'''
my_model = kpu.load(0x300000)
#anchors = [0.84, 1.22, 1.66, 2.34, 1.31, 1.75, 1.88, 2.59, 1.47, 2.09]
anchors = [3.8125, 3.8125, 5.375, 5.375, 7.1875, 7.1875, 11.25, 11.3125, 9.125, 9.125]
_ = kpu.init_yolo2(my_model, 0.5, 0.3, 5, anchors)
while True:
img = sensor.snapshot()
objects = kpu.run_yolo2(my_model,img)
if objects:
for obj in objects:
confidence = obj.value()
itemROL = obj.rect()
classID = int(obj.classid())
x, y, w, h = itemROL
# 在相应位置，标注
img.draw_rectangle(itemROL) # 画一个矩形框出目标
img.draw_cross(x+w//2, y+h//2) # 在图像中心画十字叉
img.draw_string(x, y-20,"%s : %.2f" %(labels[classID], confidence), scale=2, color=color_R)
# 标注出位置
x_offset = x+w//2 - img.width()//2 # X轴偏移量
y_offset = y+h//2 - img.height()//2 # Y轴偏移量
if x_offset > 50:
position = "right"
elif x_offset < -50:
position = "left"
else:
position = "Center"
img.draw_string(x, y+10, "%s : %d"%(position,x_offset))
else:
classID = -1
lcd.display(img) # 先显示图像
#lcd.draw_string(10, 10, "Position: " + position, lcd.WHITE, lcd.BLACK) # 再显示文字
#lcd.draw_string(10, 30, "X Offset: " + str(x_offset), lcd.WHITE, lcd.BLACK) # 显示X轴偏移
#lcd.draw_string(10, 50, "Y Offset: " + str(y_offset), lcd.WHITE, lcd.BLACK) # 显示Y轴偏移
'''