import time, os, math
from media.sensor import *
from media.display import *
from media.media import *
from machine import FPIOA, PWM, Pin
from machine import UART
from machine import TOUCH
import gc
图像参数
IMAGE_WIDTH = 800 # 增大图像宽度
IMAGE_HEIGHT = 480 # 增大图像高度
SENSOR_ID = 2
sensor = None
THRESHOLD = (69, 252) # 二值化阈值
显示模式设置
DISPLAY_MODE = “LCD” # 可选 “VIRT”, “LCD”, “HDMI”
根据显示模式设置分辨率
if DISPLAY_MODE == “VIRT”:
DISPLAY_WIDTH, DISPLAY_HEIGHT = 1920, 1080
elif DISPLAY_MODE == “LCD”:
DISPLAY_WIDTH, DISPLAY_HEIGHT = 800, 480
elif DISPLAY_MODE == “HDMI”:
DISPLAY_WIDTH, DISPLAY_HEIGHT = 1920, 1080
else:
raise ValueError(“无效的显示模式”)
角点检测阈值
CORNERS_THRESHOLD = 5000
透视变换目标尺寸
PERSPECTIVE_SIZE = 200 # 变换后的矩形大小
矩形过滤参数
MIN_AREA_RATIO = 0.01 # 最小面积占图像比例
MAX_AREA_RATIO = 0.8 # 最大面积占图像比例
ASPECT_RATIO_MIN = 0.5 # 最小宽高比
ASPECT_RATIO_MAX = 2.0 # 最大宽高比
COS_THRESHOLD = 0.3 # 角度余弦阈值(绝对值)
fpioa = FPIOA()
fpioa.set_function(11, FPIOA.UART2_TXD)
fpioa.set_function(12, FPIOA.UART2_RXD)
uart = UART(UART.UART2, 115200, 8, 0, 1, timeout=1000)
fpioa.set_function(53, FPIOA.GPIO53)
button = Pin(53, Pin.IN, Pin.PULL_DOWN) # 使用下拉电阻
debounce_delay = 20 # 毫秒
last_press_time = 0 # 上次按键按下的时间,单位为毫秒
button_last_state = 0 # 上次按键状态
timeTouch = 0
tp = TOUCH(0)
按钮文本
textL = [“back”, “LL-”, “LH-”, “AL-”, “AH-”, “BL-”, “BH-”, “set”]
textR = [“LAB”, “LL+”, “LH+”, “AL+”, “AH+”, “BL+”, “BH+”, “Ninvert”]
isLabFlag = True # 是否是LAB
isInvertFlag = False # 是否反转
虚拟按钮范围,用于触摸点判断
buttonsL = [
(0, 5, 100, 50), # 按钮"back"
(0, 65, 100, 50), # 按钮"LL-"
(0, 125, 100, 50), # 按钮"LH-"
(0, 185, 100, 50), # 按钮"AL-"
(0, 245, 100, 50), # 按钮"AH-"
(0, 305, 100, 50), # 按钮"BL-"
(0, 365, 100, 50), # 按钮"BH-"
(0, 425, 100, 50) # 按钮"set"
]
buttonsR = [
(700, 5, 100, 50), # 按钮"change"
(700, 65, 100, 50), # 按钮"LL+"(与左侧"LL-“对称)
(700, 125, 100, 50), # 按钮"LH+”(与左侧"LH-“对称)
(700, 185, 100, 50), # 按钮"AL+”(与左侧"AL-“对称)
(700, 245, 100, 50), # 按钮"AH+”(与左侧"AH-“对称)
(700, 305, 100, 50), # 按钮"BL+”(与左侧"BL-“对称)
(700, 365, 100, 50), # 按钮"BH+”(与左侧"BH-“对称)
(700, 425, 100, 50) # 按钮” "
]
buttonsS = [
(390, 5, 100, 50)
]
标准LAB范围初始化
L_MIN, L_MAX = 0, 52 # L亮度范围0-100
A_MIN, A_MAX = 31, 90 # a轴范围-128~127
B_MIN, B_MAX = -38, 109 # b轴范围-128~127
GL, GH = 0, 255 # 灰度范围0~255
step = 1 # 脱机阈值步进
LAB_thresholds = [(0, 52, 31, 90, -38, 109)] # LAB阈值列表
Gray_thresholds = [(int(GL), int(GH))] # 灰度阈值列表
sensor_id = 2
sensor = None
flag = False # 页面标志,True为阈值设置
脱机调整阈值界面设计
def changeScreen(img):
img_1 = img.copy()
screen = img.copy()
# 绘制白色背景、阈值画面范围及原始画面 screen.draw_rectangle(0, 0, DISPLAY_WIDTH, DISPLAY_HEIGHT, (255, 255, 255), fill=True) screen.draw_rectangle(200, 170, 400, 240, (0, 0, 0), 2) # 显示原始图像(全屏) screen.draw_image(img_1, 0, 0, x_size=DISPLAY_WIDTH, y_size=DISPLAY_HEIGHT) # 获取画面 if isLabFlag: LabImg = img_1.binary(LAB_thresholds) if isInvertFlag: LabImg = LabImg.invert() # 显示LAB阈值图像(全屏) screen.draw_image(LabImg, 0, 0, x_size=DISPLAY_WIDTH, y_size=DISPLAY_HEIGHT) screen.draw_string_advanced(150, 420, 30, f"L: [{L_MIN} {L_MAX}]", color=(0, 0, 0)) screen.draw_string_advanced(300, 420, 30, f"A: [{A_MIN} {A_MAX}]", color=(0, 0, 0)) screen.draw_string_advanced(500, 420, 30, f"B: [{B_MIN} {B_MAX}]", color=(0, 0, 0)) else: GrayImg = img_1.to_grayscale().binary(Gray_thresholds) if isInvertFlag: GrayImg = GrayImg.invert() # 显示灰度阈值图像(全屏) screen.draw_image(GrayImg, 0, 0, x_size=DISPLAY_WIDTH, y_size=DISPLAY_HEIGHT) screen.draw_string_advanced(150, 420, 30, f"Gray: [{GL} {GH}]", color=(0, 0, 0)) # 绘制虚拟按钮,编写文本 for i in range(8): screen.draw_rectangle(0, 5+i*60, 100, 50, (200, 201, 202), thickness=1, fill=True) screen.draw_string_advanced(30, 20+i*60, 20, textL[i], (0, 0, 0)) screen.draw_rectangle(700, 5+i*60, 100, 50, (200, 201, 202), thickness=1, fill=True) screen.draw_string_advanced(730, 20+i*60, 20, textR[i], (0, 0, 0)) screen.draw_rectangle(390, 5, 100, 50, (200, 201, 202), thickness=1, fill=True) screen.draw_string_advanced(400, 20, 20, f"step: {step}", (0, 0, 0)) # 显示处理后的图像 Display.show_image(screen, 0, 0)
触摸屏按钮事件动作
def buttonAction(direction, index):
# global获取全局变量,可更改数据
global L_MAX, L_MIN, A_MAX, A_MIN, B_MAX, B_MIN, flag, step
global isLabFlag, textR, GL, GH, LAB_thresholds, isInvertFlag, Gray_thresholds
# 阈值列表重置 LAB_thresholds = [(int(L_MIN), int(L_MAX), int(A_MIN), int(A_MAX), int(B_MIN), int(B_MAX))] Gray_thresholds = [(int(GL), int(GH))] # 按钮处理 if direction == 'left': # 减操作 if index == 1: if isLabFlag: L_MIN = max(L_MIN - step, 0) # 确保L_MIN >= 0 else: GL = max(GL - step, 0) elif index == 2: if isLabFlag: L_MAX = max(L_MAX - step, 0) else: GH = max(GH - step, 0) elif index == 3: A_MIN = max(A_MIN - step, -128) elif index == 4: A_MAX = max(A_MAX - step, -128) elif index == 5: B_MIN = max(B_MIN - step, -128) elif index == 6: B_MAX = max(B_MAX - step, -128) elif index == 0: flag = False elif direction == 'right': # 加操作 if index == 1: if isLabFlag: L_MIN = min(L_MIN + step, 100) # 确保L_MIN <= 100 else: GL = min(GL + step, 255) elif index == 2: if isLabFlag: L_MAX = min(L_MAX + step, 100) else: GH = min(GH + step, 255) elif index == 3: A_MIN = min(A_MIN + step, 127) elif index == 4: A_MAX = min(A_MAX + step, 127) elif index == 5: B_MIN = min(B_MIN + step, 127) elif index == 6: B_MAX = min(B_MAX + step, 127) elif index == 7: isInvertFlag = not isInvertFlag if isInvertFlag: textR[7] = 'invert' # 更改文本 else: textR[7] = 'Ninvert' elif index == 0: isLabFlag = not isLabFlag if isLabFlag: textR[0] = 'LAB' textL[1] = 'LL-' textR[1] = 'LL+' textL[2] = 'LH-' textR[2] = 'LH+' else: textR[0] = 'Gray' textL[1] = 'GL-' textR[1] = 'GL+' textL[2] = 'GH-' textR[2] = 'GH+' elif direction == 'step': # 调整步进 step += 1 if step > 5: step = 1
触摸屏事件
def touchAction():
global timeTouch
touchP = tp.read(2)
# 每3次计为1次,减少sleep时间,确保摄像头画面流畅 if touchP: timeTouch += 1 if timeTouch >= 3: p = touchP timeTouch = 0 if p: x, y = p[0].x, p[0].y # 通过触摸坐标与按钮覆盖位置确定触摸按钮 if x < 200: for i, (rect_x, rect_y, rect_w, rect_h) in enumerate(buttonsL): if (rect_x <= x <= rect_x + rect_w) and (rect_y <= y <= rect_y + rect_h): print(f"操作: {textL[i]}") buttonAction('left', i) elif x > 600: for i, (rect_x, rect_y, rect_w, rect_h) in enumerate(buttonsR): if (rect_x <= x <= rect_x + rect_w) and (rect_y <= y <= rect_y + rect_h): print(f"操作: {textR[i]}") buttonAction('right', i) else: for i, (rect_x, rect_y, rect_w, rect_h) in enumerate(buttonsS): if (rect_x <= x <= rect_x + rect_w) and (rect_y <= y <= rect_y + rect_h): print(f"操作: step") buttonAction('step', 1) time.sleep(0.01)
def send_coordinates(center):
“”“通过串口发送坐标数据”“”
if center:
cx, cy = center
# 协议格式: $X[坐标x]Y[坐标y]\n
data_str = f"$X{cx:04d}Y{cy:04d}\n"
uart.write(data_str.encode())
else:
uart.write(b"$X9999Y9999\n") # 无效坐标标志
def sort_corners(corners):
“”“对矩形角点进行排序(左上、右上、右下、左下)”“”
if len(corners) != 4:
return corners
# 计算中心点 center_x = sum(c[0] for c in corners) / 4 center_y = sum(c[1] for c in corners) / 4 # 分类角点 top_left = min(corners, key=lambda c: (c[0] - center_x)**2 + (c[1] - center_y)**2 if c[0] < center_x and c[1] < center_y else float('inf')) top_right = min(corners, key=lambda c: (c[0] - center_x)**2 + (c[1] - center_y)**2 if c[0] > center_x and c[1] < center_y else float('inf')) bottom_right = min(corners, key=lambda c: (c[0] - center_x)**2 + (c[1] - center_y)**2 if c[0] > center_x and c[1] > center_y else float('inf')) bottom_left = min(corners, key=lambda c: (c[0] - center_x)**2 + (c[1] - center_y)**2 if c[0] < center_x and c[1] > center_y else float('inf')) return [top_left, top_right, bottom_right, bottom_left]
def matrix_multiply(A, B):
“”“3x3矩阵乘法”“”
result = [[0, 0, 0], [0, 0, 0], [0, 0, 0]]
for i in range(3):
for j in range(3):
for k in range(3):
result[i][j] += A[i][k] * B[k][j]
return result
def matrix_vector_multiply(M, v):
“”“矩阵向量乘法”“”
result = [0, 0, 0]
for i in range(3):
for j in range(3):
result[i] += M[i][j] * v[j]
return result
def matrix_inverse(M):
“”“3x3矩阵求逆”“”
det = (M[0][0] * (M[1][1]*M[2][2] - M[1][2]*M[2][1]) -
M[0][1] * (M[1][0]*M[2][2] - M[1][2]*M[2][0]) +
M[0][2] * (M[1][0]*M[2][1] - M[1][1]*M[2][0]))
if abs(det) < 1e-10: # 行列式接近0,不可逆 return [[1, 0, 0], [0, 1, 0], [0, 0, 1]] inv_det = 1.0 / det inv = [ [(M[1][1]*M[2][2] - M[2][1]*M[1][2]) * inv_det, (M[0][2]*M[2][1] - M[0][1]*M[2][2]) * inv_det, (M[0][1]*M[1][2] - M[0][2]*M[1][1]) * inv_det], [(M[1][2]*M[2][0] - M[1][0]*M[2][2]) * inv_d极, (M[0][0]*M[2][2] - M[0][2]*M[2][0]) * inv_det, (M[1][0]*M[0][2] - M[0][0]*M[1][2]) * inv_det], [(M[1][0]*M[2][1] - M[2][0]*M[1][1]) * inv_det, (M[2][0]*M[0][1] - M[0][0]*M[2][1]) * inv_det, (M[0][0]*M[1][1] - M[1][0]*M[0][1]) * inv_det] ] return inv
def calculate_perspective_transform(src_points):
“”"
计算透视变换矩阵
src_points: 源图像中的四个点 [左上, 右上, 右下, 左下]
返回: 变换矩阵和逆变换矩阵
“”"
# 目标点 (校正后的正方形)
dst_points = [
[0, 0],
[PERSPECTIVE_SIZE, 0],
[PERSPECTIVE_SIZE, PERSPECTIVE_SIZE],
[0, PERSPECTIVE_SIZE]
]
# 构建A矩阵 A = [] for i in range(4): x, y = src_points[i] u, v = dst_points[i] A.append([x, y, 1, 0, 0, 0, -u*x, -u*y]) A.append([0, 0, 0, x, y, 1, -v*x, -v*y]) # 构建b向量 b = [] for i in range(4): u, v = dst_points[i] b.append(u) b.append(v) # 解线性方程组 (使用最小二乘法简化) h = [0] * 8 for i in range(8): for j in range(8): # 计算A^T * A ata = 0 for k in range(8): ata += A[k][i] * A[k][j] # 计算A^T * b atb = 0 for k in range(8): atb += A[k][i] * b[k] # 简单求解 if abs(ata) > 1e-10: h[i] = atb / ata # 构造变换矩阵 H = [ [h[0], h[1], h[2]], [h[3], h[4], h[5]], [h[6], h[7], 1] ] # 计算逆矩阵 H_inv = matrix_inverse(H) return H, H_inv
def apply_perspective_transform(point, H):
“”“应用透视变换到单个点”“”
x, y = point
src = [x, y, 1]
dst = matrix_vector_multiply(H, src)
if abs(dst[2]) > 1e-10:
dst = [dst[0]/dst[2], dst[1]/dst[2]]
return dst
def calculate_center(corners):
“”“计算矩形中心点(使用透视校正)”“”
if len(corners) < 4:
return None
# 计算几何中心(原始中心) raw_center_x = sum(c[0] for c in corners) / len(corners) raw_center_y = sum(c[1] for c in corners) / len(corners) # 计算透视变换 try: H, H_inv = calculate_perspective_transform(corners) # 将中心点映射到透视校正后的空间 corrected_center = apply_perspective_transform((raw_center_x, raw_center_y), H) # 将校正后的中心点映射回原始图像空间 final_center = apply_perspective_transform(corrected_center, H_inv) return (int(final_center[0]), int(final_center[1])) except Exception as e: # 计算失败时使用原始中心 return (int(raw_center_x), int(raw_center_y))
def is_valid_rectangle(corners, cos_threshold=COS_THRESHOLD):
“”"
检查是否为有效矩形(内角接近90度)
corners: 排序后的四个角点 [左上, 右上, 右下, 左下]
cos_threshold: 余弦值阈值(绝对值应小于此值)
返回: True/False
“”"
if len(corners) != 4:
return False
# 计算四个内角的余弦值 for i in range(4): # 获取三个连续点: A-B-C A = corners[i] B = corners[(i + 1) % 4] C = corners[(i + 2) % 4] # 计算向量BA和BC BA = (A[0] - B[0], A[1] - B[1]) BC = (C[0] - B[0], C[1] - B[1]) # 计算点积和模长 dot_product = BA[0] * BC[0] + BA[1] * BC[1] mod_BA = math.sqrt(BA[0]**2 + BA[1]**2) mod_BC = math.sqrt(BC[0]**2 + BC[1]**2) # 避免除零错误 if mod_BA * mod_BC < 1e-5: return False # 计算余弦值 cos_value = dot_product / (mod_BA * mod_BC) # 检查是否接近90度 (|cos| < threshold) if abs(cos_value) > cos_threshold: return False return True
def calculate_rect_properties(corners):
“”“计算矩形的面积和宽高极”“”
if len(corners) != 4:
return 0, 0
# 计算边长 top = math.sqrt((corners[1][0] - corners[0][0])**2 + (corners[1][1] - corners[0][1])**2) right = math.sqrt((corners[2][0] - corners[1][0])**2 + (corners[2][1] - corners[1][1])**2) bottom = math.sqrt((corners[3][0] - corners[2][0])**2 + (corners[3][1] - corners[2][1])**2) left = math.sqrt((corners[0][0] - corners[3][0])**2 + (corners[0][1] - corners[3][1])**2) # 计算平均宽度和高度 width = (top + bottom) / 2 height = (left + right) / 2 # 计算面积和宽高比 area = width * height aspect_ratio = max(width, height) / min(width, height) if min(width, height) > 0 else 0 return area, aspect_ratio
def draw_corner_info(img, corners, center, area, aspect_ratio):
“”“在图像上绘制角点和中心信息”“”
if len(corners) != 4:
return
# 定义角点颜色和标签 corner_colors = [(0, 255, 0), (0, 255, 0), (0, 255, 0), (0, 255, 0)] labels = ["TL", "TR", "BR", "BL"] # 绘制角点 for i, (x, y) in enumerate(corners): color = corner_colors[i] img.draw_circle(int(x), int(y), 8, color=color, thickness=2) img.draw_string(int(x) + 10, int(y) - 10, labels[i], color=color, scale=1) # 绘制矩形边框 for i in range(4): x1, y1 = corners[i] x2, y2 = corners[(i + 1) % 4] img.draw_line(int(x1), int(y1), int(x2), int(y2), color=(0, 255, 255), thickness=2) # 绘制对角线 img.draw_line(int(corners[0][0]), int(corners[0][1]), int(corners[2][0]), int(corners[2][1]), color=(255, 0, 0), thickness=1) img.draw_line(int(corners[1][0]), int(corners[1][1]), int(corners[3][0]), int(corners[3][1]), color=(255, 0, 0), thickness=1) # 绘制中心点 if center: cx, cy = center img.draw_cross(int(cx), int(cy), size=15, color=(255, 0, 255), thickness=2) img.draw_circle(int(cx), int(cy), 5, color=(255, 0, 255), thickness=-1) img.draw_string(int(cx) + 10, int(cy) - 10, "Center", color=(255, 0, 255), scale=1) # 显示矩形属性 img.draw_string(10, 30, f"Area: {area:.1f} px", color=(0, 255, 0), scale=1) img.draw_string(10, 50, f"Aspect: {aspect_ratio:.2f}", color=(0, 255, 0), scale=1)
def process_frame(img):
“”“处理图像帧,检测矩形角点和中心(带形状验证)”“”
img_gray = img.to_grayscale(copy=False)
img_bin = img_gray.binary([THRESHOLD])
rects = img_bin.find_rects(threshold=CORNERS_THRESHOLD)
# 计算图像总面积 total_image_area = img.width() * img.height() min_area = MIN_AREA_RATIO * total_image_area max_area = MAX_AREA_RATIO * total_image_area # 找出所有可能的矩形 valid_rects = [] for rect in rects: corners = rect.corners() if len(corners) == 4: sorted_corners = sort_corners(corners) # 计算矩形属性 area, aspect_ratio = calculate_rect_properties(sorted_corners) # 检查角度、面积和宽高比 angle_valid = is_valid_rectangle(sorted_corners) area_valid = min_area <= area <= max_area aspect_valid = ASPECT_RATIO_MIN <= aspect_ratio <= ASPECT_RATIO_MAX if angle_valid and area_valid and aspect_valid: valid_rects.append((area, aspect_ratio, sorted_corners)) # 选择面积最大的有效矩形 corners = [] center = None area = 0 aspect_ratio = 0 if valid_rects: valid_rects.sort(key=lambda x: x[0], reverse=True) area, aspect_ratio, corners = valid_rects[0] center = calculate_center(corners) # 绘制结果 if center: draw_corner_info(img, corners, center, area, aspect_ratio) else: # 显示未检测到矩形的信息 img.draw_string(img.width()//2 - 50, img.height()//2, "No valid rectangle", color=(255, 0, 0), scale=1) # 资源清理 del img_gray, img_bin gc.collect() return corners, center
try:
print(“初始化摄像头…”)
# 初始化摄像头
sensor = Sensor(id=SENSOR_ID)
sensor.reset()
# 设置与显示匹配的分辨率
sensor.set_framesize(width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT)
sensor.set_pixformat(Sensor.RGB565, chn=CAM_CHN_ID_0)
sensor.set_hmirror(1)
sensor.set_vflip(1)
print("初始化显示器...") # 初始化显示器 if DISPLAY_MODE == "VIRT": Display.init(Display.VIRT, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, fps=60) elif DISPLAY_MODE == "LCD": Display.init(Display.ST7701, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, to_ide=True) elif DISPLAY_MODE == "HDMI": Display.init(Display.LT9611, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, to_ide=True) print("初始化媒体管理器...") # 初始化媒体管理器 MediaManager.init() sensor.run() clock = time.clock() print("开始主循环...") while True: os.exitpoint() clock.tick() # 捕获图像 img = sensor.snapshot(chn=CAM_CHN_ID_0) # 处理按键事件 button_state = button.value() # 获取当前按键状态 current_time = time.ticks_ms() # 获取当前时间(单位:毫秒) # 检测按键从未按下(0)到按下(1)的变化(上升沿) if button_state == 1 and button_last_state == 0: # 检查按键是否在消抖时间外 if current_time - last_press_time > debounce_delay: if button.value() == 1: flag = not flag last_press_time = current_time # 更新按键按下时间 # 更新上次按键状态 button_last_state = button_state # 仅在阈值设置界面显示阈值调整UI if flag: changeScreen(img) touchAction() else: # 正常模式下显示原始图像和处理结果 # 处理图像并检测角点 corners, center = process_frame(img) # 发送坐标 send_coordinates(center) time.sleep_ms(20) # 控制发送频率 # 显示FPS img.draw_string(10, 10, f"FPS: {clock.fps():.1f}", color=(255, 0, 0), scale=1) # 显示图像(全屏) img.compressed_for_ide() Display.show_image(img, 0, 0) os.exitpoint() # 可用的退出点
except KeyboardInterrupt:
print(“程序被用户中断”)
except Exception as e:
print(f"发生错误: {e}")
import sys
sys.print_exception(e)
finally:
print(“清理资源…”)
# 清理资源
if sensor:
sensor.stop()
Display.deinit()
MediaManager.deinit()
print(“程序退出”)
将上述程序的lab阈值调节改成灰度阈值调节基本功能不变通过板载按键(配置为此IO口fpioa.set_function(53, FPIOA.GPIO53))控制界面进入阈值调参界面:能够调节阈值,并且添加一个新功能:第二次按下板载按键时启用一个新功能为k230通过UART3不断向stm32的另一个串口发送一个信号,当stm32收到信号时启用一个新函数,stm32函数功能为:收到信号后,先控制舵机在当前舵机的位置旋转一圈去找矩形框,在此过程中当stm32收到k230的UART2发送的坐标即k230找到了矩形靶时,立刻控制舵机对准目标然后实现点亮激光灯打靶,如下是stm32端的main.c和现有的uart.c uart.h代码(stm32端新的uart口在uart.c uart.h中配置,并在main.c中使用,stm32新的功能函数用.c .h 文件配置,并在main.c中调用),写出k230与stm32双端的修改补充后的完整代码#include "stm32f10x.h"
#include "pwm.h"
#include "uart.h"
#include "pid.h"
#include "Delay.h"
uint8_t first_valid_received = 0; // 首次有效坐标标志
uint16_t laser_delay_count = 0; // 激光灯延时计数器
// 图像参数(需与K230一致)
#define IMG_WIDTH 352
#define IMG_HEIGHT 240
#define CENTER_X (IMG_WIDTH/2)
#define CENTER_Y (IMG_HEIGHT/2)
#define JIGUANG_X 172
#define JIGUANG_Y 137
// 矩形状态枚举
typedef enum {
VALID_RECT, // 有效矩形(4条边框完整)
MISS_ONE_EDGE, // 缺失一条边框
ONLY_ONE_EDGE, // 只有一条边框
UNKNOWN_STATE // 未知状态
} RectState;
// 扫描模式步骤
typedef enum {
SCAN_LEFT,
SCAN_RIGHT,
SCAN_UP,
SCAN_DOWN,
SCAN_STOP
} ScanStep;
// PID控制器实例
PID_Controller pid_ud, pid_lr;
// 全局状态变量
RectState current_state = UNKNOWN_STATE;
ScanStep scan_step = SCAN_STOP;
uint8_t scan_count = 0; // 扫描计数(控制步骤切换频率)
const uint8_t SCAN_DELAY = 20; // 每步扫描延迟计数
const float STEP_ANGLE = 3.0f; // 扫描步长(度)
// 激光灯控制引脚定义
#define LASER_PIN GPIO_Pin_0
#define LASER_PORT GPIOA
// 激光灯状态
uint8_t laser_enabled = 1; // 1=关闭, 0=开启 (初始关闭)
// 初始化激光灯控制引脚
void Laser_Init(void) {
GPIO_InitTypeDef GPIO_InitStructure;
// 启用GPIOA时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
// 配置PA0为推挽输出
GPIO_InitStructure.GPIO_Pin = LASER_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(LASER_PORT, &GPIO_InitStructure);
// 初始状态关闭激光灯
GPIO_SetBits(LASER_PORT, LASER_PIN);
laser_enabled = 1; // 初始状态为关闭
}
// 控制激光灯状态
void Set_Laser(uint8_t state) {
if (state) {
GPIO_SetBits(LASER_PORT, LASER_PIN); // 开启激光灯
laser_enabled = 1;
} else {
GPIO_ResetBits(LASER_PORT, LASER_PIN); // 关闭激光灯
laser_enabled = 0;
}
}
// 舵机初始化函数 - 确保舵机正确归位
void Servo_Init(void) {
// 设置舵机到安全位置
Set_Servo_Angle(TIM3, 1, 180.0f); // 上下舵机居中
Set_Servo_Angle(TIM3, 2, 70.0f); // 左右舵机居中
// 给舵机时间归位
Delay_ms(500);
}
int main(void) {
// 初始化系统时钟
SystemInit();
// 初始化前延时,确保电源稳定
Delay_ms(80);
// 初始化外设
PWM_Init(); // 先初始化PWM
USART1_Init(); // 初始化串口
Laser_Init(); // 初始化激光灯控制
// 初始化PID控制器
PID_Init(&pid_ud, 0.15, 0.001, 0.05, 30); // 上下舵机PID
PID_Init(&pid_lr, 0.33, 0.002, 0.45, 50); // 左右舵机PID
// 初始化舵机位置 - 添加专门的初始化函数
Servo_Init();
// 主循环变量
float angle_ud = 97.0f, angle_lr = 75.0f;
while(1) {
if(data_ready) {
int target_x = 0, target_y = 0;
char *ptr = (char*)rx_buffer;
// 解析K230发送的协议数据
if(sscanf(ptr, "X%dY%d", &target_x, &target_y) == 2) {
// 判断矩形状态
if(target_x == 8888 && target_y == 8888) {
current_state = MISS_ONE_EDGE;
scan_step = SCAN_LEFT; // 从左扫描开始
scan_count = 0;
}
else if(target_x == 7777 && target_y == 7777) {
current_state = ONLY_ONE_EDGE;
scan_step = SCAN_LEFT; // 从左扫描开始
scan_count = 0;
}
else if(target_x == 9999 || target_y == 9999) {
current_state = UNKNOWN_STATE;
}
else {
current_state = VALID_RECT;
scan_step = SCAN_STOP; // 停止扫描
// 有效矩形时执行PID对准
float dx = JIGUANG_X - target_x;
float dy = JIGUANG_Y - target_y;
// 更新PID(转换为角度控制)
angle_lr += PID_Update(&pid_lr, 0, dx) * 0.21052;
angle_ud += PID_Update(&pid_ud, 0, dy) * 0.23529;
// 角度限幅
angle_lr = (angle_lr < 0) ? 0 : (angle_lr > 360) ? 360 : angle_lr;
angle_ud = (angle_ud < 75) ? 75 : (angle_ud >105) ? 105 : angle_ud;
// 更新舵机位置
Set_Servo_Angle(TIM3, 1, angle_ud);
Set_Servo_Angle(TIM3, 2, angle_lr);
// 首次有效坐标处理
if (!first_valid_received) {
first_valid_received = 1; // 标记已接收
laser_delay_count = 350;// 设置300ms延时计数
// Set_Laser(0); // 开启激光灯(低电平点亮)
}
}
}
data_ready = 0; // 清除数据就绪标志
}
// +++ 新增: 激光灯延时控制 +++
if (laser_delay_count > 0) {
laser_delay_count--; // 递减计数器
if (laser_delay_count == 0) {
// 延时结束,点亮激光灯
Set_Laser(0); // 低电平点亮
}
}
// 处理扫描逻辑(非有效矩形状态)
if(current_state != VALID_RECT && scan_step != SCAN_STOP) {
scan_count++;
if(scan_count >= SCAN_DELAY) { // 延迟一定时间再切换步骤
scan_count = 0;
// 根据不同状态设置扫描范围(只有一条边框时扫描范围更大)
float max_left = (current_state == ONLY_ONE_EDGE) ? 30.0f : 60.0f;
float max_right = (current_state == ONLY_ONE_EDGE) ? 150.0f : 120.0f;
float max_up = (current_state == ONLY_ONE_EDGE) ? 30.0f : 60.0f;
float max_down = (current_state == ONLY_ONE_EDGE) ? 150.0f : 120.0f;
// 执行当前步骤的扫描动作
switch(scan_step) {
case SCAN_LEFT:
angle_lr -= STEP_ANGLE;
if(angle_lr <= max_left) {
scan_step = SCAN_RIGHT; // 到达左极限,切换到右移
}
break;
case SCAN_RIGHT:
angle_lr += STEP_ANGLE;
if(angle_lr >= max_right) {
scan_step = SCAN_UP; // 到达右极限,切换到上移
}
break;
case SCAN_UP:
angle_ud -= STEP_ANGLE;
if(angle_ud <= max_up) {
scan_step = SCAN_DOWN; // 到达上极限,切换到下移
}
break;
case SCAN_DOWN:
angle_ud += STEP_ANGLE;
if(angle_ud >= max_down) {
scan_step = SCAN_LEFT; // 到达下极限,回到左移循环
}
break;
default:
break;
}
// 确保角度在安全范围内
angle_lr = (angle_lr < 0) ? 0 : (angle_lr > 360) ? 360: angle_lr;
angle_ud = (angle_ud < 75) ? 75 : (angle_ud > 105) ? 105 : angle_ud;
// 更新舵机位置
Set_Servo_Angle(TIM3, 1, angle_ud);
Set_Servo_Angle(TIM3, 2, angle_lr);
}
}
// 添加短暂延时,确保系统稳定
Delay_ms(1);
}
}
#include "uart.h"
volatile uint8_t rx_buffer[RX_BUF_SIZE] = {0};
volatile uint8_t rx_index = 0;
volatile uint8_t data_ready = 0;
void USART1_Init(void) {
// 1. 开启时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 | RCC_APB2Periph_GPIOA, ENABLE);
// 2. 配置GPIO
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; // TX(PA9)
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; // RX(PA10)
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// 3. 配置串口参数
USART_InitTypeDef USART_InitStructure;
USART_InitStructure.USART_BaudRate = 115200;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART1, &USART_InitStructure);
// 4. 配置中断
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
NVIC_EnableIRQ(USART1_IRQn);
// 5. 启动串口
USART_Cmd(USART1, ENABLE);
}
// 串口中断服务函数
void USART1_IRQHandler(void) {
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) {
uint8_t ch = USART_ReceiveData(USART1);
if(ch == '$') { // 帧开始
rx_index = 0;
}
else if(ch == '\n') { // 帧结束
rx_buffer[rx_index] = '\0';
data_ready = 1;
}
else if(rx_index < RX_BUF_SIZE - 1) {
rx_buffer[rx_index++] = ch;
}
}
}
#ifndef __UART_H
#define __UART_H
#include "stm32f10x.h"
#define RX_BUF_SIZE 32
void USART1_Init(void);
void USART1_SendChar(char ch);
void USART1_SendString(char *str);
extern volatile uint8_t rx_buffer[RX_BUF_SIZE];
extern volatile uint8_t rx_index;
extern volatile uint8_t data_ready;
#endif
最新发布