542 lines
18 KiB
Python
542 lines
18 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""
|
||
激光中心点检测单元测试（单文件，无项目依赖）
|
||
直接使用 maix 标准库，实现红色激光点坐标检测
|
||
|
||
运行方式：
|
||
python3 test/test_laser_center_point.py
|
||
|
||
Ctrl+C 退出，按 s 保存截图
|
||
"""
|
||
|
||
from maix import camera, display, image, time, app, uart, pinmap
|
||
import os
|
||
import struct
|
||
import select
|
||
|
||
_USE_CV = False
|
||
try:
|
||
import cv2
|
||
import numpy as np
|
||
_USE_CV = True
|
||
except ImportError:
|
||
pass
|
||
|
||
WIDTH = 640
|
||
HEIGHT = 480
|
||
THRESHOLD = 140
|
||
SEARCH_RADIUS = 50
|
||
|
||
|
||
def read_key_ev():
|
||
"""非阻塞读取 /dev/input/event0 按键（返回 key_code 或 -1）"""
|
||
try:
|
||
r, _, _ = select.select([_key_fd], [], [], 0)
|
||
if r:
|
||
event = _key_fd.read(16)
|
||
if len(event) == 16:
|
||
_, _, etype, code, value = struct.unpack("IIHHI", event)
|
||
if etype == 1 and value == 1:
|
||
return code
|
||
except Exception:
|
||
pass
|
||
return -1
|
||
|
||
|
||
def find_ellipse(img_cv, cx, cy, roi_r, th):
|
||
x1 = max(0, cx - roi_r)
|
||
x2 = min(WIDTH, cx + roi_r)
|
||
y1 = max(0, cy - roi_r)
|
||
y2 = min(HEIGHT, cy + roi_r)
|
||
roi = img_cv[y1:y2, x1:x2]
|
||
if roi.size == 0:
|
||
return None
|
||
r = roi[:, :, 0].astype(np.int32)
|
||
g = roi[:, :, 1].astype(np.int32)
|
||
b = roi[:, :, 2].astype(np.int32)
|
||
mask = (r > th) & (r > g * 1.5) & (r > b * 1.5)
|
||
oe = (r > 200) & (g > 200) & (b > 200) & (r >= g) & (r >= b) & ((r - g) > 10) & ((r - b) > 10)
|
||
combined = (mask | oe).astype(np.uint8) * 255
|
||
contours, _ = cv2.findContours(combined, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||
if not contours:
|
||
return None
|
||
largest = max(contours, key=cv2.contourArea)
|
||
if cv2.contourArea(largest) < 5:
|
||
return None
|
||
cnt = largest.copy()
|
||
for pt in cnt:
|
||
pt[0][0] += x1
|
||
pt[0][1] += y1
|
||
if len(cnt) >= 5:
|
||
(ex, ey), (ew, eh), ang = cv2.fitEllipse(cnt)
|
||
mask_ellipse = np.zeros((HEIGHT, WIDTH), dtype=np.uint8)
|
||
cv2.ellipse(mask_ellipse, (int(ex), int(ey)), (int(ew / 2), int(eh / 2)), ang, 0, 360, 255, -1)
|
||
brightness = img_cv[:, :, 0].astype(np.int32) + img_cv[:, :, 1].astype(np.int32) + img_cv[:, :, 2].astype(np.int32)
|
||
masked = np.where(mask_ellipse > 0, brightness, 0)
|
||
vals = masked[masked > 0]
|
||
if len(vals) > 0:
|
||
bth = np.percentile(vals, 90)
|
||
bmask = (masked >= bth).astype(np.uint8) * 255
|
||
bcontours, _ = cv2.findContours(bmask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||
if bcontours:
|
||
blargest = max(bcontours, key=cv2.contourArea)
|
||
if cv2.contourArea(blargest) >= 3 and len(blargest) >= 5:
|
||
(ix, iy), _, _ = cv2.fitEllipse(blargest)
|
||
return (float(ix), float(iy))
|
||
M = cv2.moments(blargest)
|
||
if M["m00"] > 0:
|
||
return (float(M["m10"] / M["m00"]), float(M["m01"] / M["m00"]))
|
||
return (float(ex), float(ey))
|
||
M = cv2.moments(cnt)
|
||
if M["m00"] > 0:
|
||
return (float(M["m10"] / M["m00"]), float(M["m01"] / M["m00"]))
|
||
return None
|
||
|
||
|
||
def find_brightest(img_cv, cx, cy, roi_r, th):
|
||
x1 = max(0, cx - roi_r)
|
||
x2 = min(WIDTH, cx + roi_r)
|
||
y1 = max(0, cy - roi_r)
|
||
y2 = min(HEIGHT, cy + roi_r)
|
||
best_score = 0
|
||
best_pos = None
|
||
for y in range(y1, y2):
|
||
for x in range(x1, x2):
|
||
r, g, b = int(img_cv[y, x, 0]), int(img_cv[y, x, 1]), int(img_cv[y, x, 2])
|
||
is_red = (r > th and r > g * 1.5 and r > b * 1.5)
|
||
is_oe = (r > 200 and g > 200 and b > 200 and r >= g and r >= b and (r - g) > 10 and (r - b) > 10)
|
||
if is_red or is_oe:
|
||
score = r + g + b
|
||
dx, dy = x - cx, y - cy
|
||
dist = (dx * dx + dy * dy) ** 0.5
|
||
score *= max(0.5, 1.0 - (dist / roi_r) * 0.5)
|
||
if score > best_score:
|
||
best_score = score
|
||
best_pos = (float(x), float(y))
|
||
return best_pos
|
||
|
||
|
||
# 打开键盘输入设备
|
||
_key_fd = None
|
||
try:
|
||
_key_fd = open("/dev/input/event0", "rb")
|
||
except Exception:
|
||
try:
|
||
_key_fd = open("/dev/input/event1", "rb")
|
||
except Exception:
|
||
_key_fd = None
|
||
|
||
print("=" * 50)
|
||
print("激光中心点检测单元测试")
|
||
print("=" * 50)
|
||
print()
|
||
|
||
cam = camera.Camera(WIDTH, HEIGHT)
|
||
disp = display.Display()
|
||
print("[OK] 摄像头和显示初始化完成")
|
||
|
||
# 初始化激光串口
|
||
_laser_on = False
|
||
_laser_uart = None
|
||
try:
|
||
pinmap.set_pin_function("A18", "UART1_RX")
|
||
pinmap.set_pin_function("A19", "UART1_TX")
|
||
_laser_uart = uart.UART("/dev/ttyS1", 9600)
|
||
_laser_uart.read(-1)
|
||
print("[OK] 激光串口初始化完成")
|
||
except Exception as e:
|
||
print(f"[WARN] 激光串口初始化失败: {e}")
|
||
|
||
LASER_ON = bytes([0xAA, 0x00, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x01, 0xC1])
|
||
LASER_OFF = bytes([0xAA, 0x00, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x00, 0xC0])
|
||
|
||
# 默认开启激光
|
||
if _laser_uart:
|
||
try:
|
||
_laser_uart.write(LASER_ON)
|
||
time.sleep_ms(50)
|
||
_laser_uart.read(-1)
|
||
_laser_on = True
|
||
print("[OK] 激光已开启")
|
||
except Exception as e:
|
||
print(f"[WARN] 开启激光失败: {e}")
|
||
print()
|
||
|
||
pos_ellipse = None
|
||
pos_bright = None
|
||
frame_count = 0
|
||
use_ellipse = True
|
||
|
||
while not app.need_exit():
|
||
frame = cam.read()
|
||
if frame is None:
|
||
time.sleep_ms(10)
|
||
continue
|
||
|
||
frame_count += 1
|
||
|
||
if _USE_CV:
|
||
img_cv = image.image2cv(frame, False, False)
|
||
cx, cy = WIDTH // 2, HEIGHT // 2
|
||
|
||
t0 = time.ticks_ms()
|
||
pos_ellipse = find_ellipse(img_cv, cx, cy, SEARCH_RADIUS, THRESHOLD)
|
||
t1 = time.ticks_ms()
|
||
pos_bright = find_brightest(img_cv, cx, cy, SEARCH_RADIUS, THRESHOLD)
|
||
t2 = time.ticks_ms()
|
||
|
||
dt_e = abs(time.ticks_diff(t0, t1))
|
||
dt_b = abs(time.ticks_diff(t1, t2))
|
||
|
||
if frame_count % 5 == 0:
|
||
e_str = f"({pos_ellipse[0]:.1f},{pos_ellipse[1]:.1f})" if pos_ellipse else "None"
|
||
b_str = f"({pos_bright[0]:.1f},{pos_bright[1]:.1f})" if pos_bright else "None"
|
||
print(f"[LASER] ellipse={e_str} ({dt_e}ms) brightest={b_str} ({dt_b}ms) "
|
||
f"th={THRESHOLD} radius={SEARCH_RADIUS}")
|
||
|
||
# 叠加显示
|
||
pos = pos_ellipse if use_ellipse else pos_bright
|
||
h, w = img_cv.shape[:2]
|
||
cv2.circle(img_cv, (cx, cy), SEARCH_RADIUS, (0, 255, 0), 1)
|
||
cv2.circle(img_cv, (cx, cy), 2, (0, 255, 0), -1)
|
||
if pos:
|
||
x, y = int(pos[0]), int(pos[1])
|
||
cv2.circle(img_cv, (x, y), 6, (0, 0, 255), 2)
|
||
cv2.line(img_cv, (x - 14, y), (x + 14, y), (0, 0, 255), 1)
|
||
cv2.line(img_cv, (x, y - 14), (x, y + 14), (0, 0, 255), 1)
|
||
cv2.putText(img_cv, f"({x},{y})", (x + 10, y - 10),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
|
||
info = [
|
||
f"pos={pos if pos else 'None'}",
|
||
f"method={'ellipse' if use_ellipse else 'brightest'} th={THRESHOLD}",
|
||
f"laser={'ON' if _laser_on else 'OFF'}",
|
||
]
|
||
for i, line in enumerate(info):
|
||
cv2.putText(img_cv, line, (8, 20 + i * 22),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1, cv2.LINE_AA)
|
||
|
||
display_frame = image.cv2image(img_cv, False, False)
|
||
else:
|
||
display_frame = frame
|
||
|
||
disp.show(display_frame)
|
||
|
||
# 按键处理（非阻塞）
|
||
key = read_key_ev()
|
||
if key > 0:
|
||
c = chr(key & 0xFF) if key < 256 else ""
|
||
if key == 113 or key == 81 or key == 0x1b: # q/Q/ESC
|
||
break
|
||
if c == "e" or key == 18: # e
|
||
use_ellipse = not use_ellipse
|
||
print(f"[KEY] Method: {'ellipse' if use_ellipse else 'brightest'}")
|
||
if c == "l" or key == 12: # l
|
||
_laser_on = not _laser_on
|
||
if _laser_uart:
|
||
try:
|
||
_laser_uart.write(LASER_ON if _laser_on else LASER_OFF)
|
||
time.sleep_ms(30)
|
||
_laser_uart.read(-1)
|
||
print(f"[KEY] Laser: {'ON' if _laser_on else 'OFF'}")
|
||
except Exception as e:
|
||
print(f"[KEY] Laser error: {e}")
|
||
else:
|
||
print("[KEY] Laser UART not available")
|
||
time.sleep_ms(30)
|
||
|
||
# 关闭激光
|
||
if _laser_on and _laser_uart:
|
||
try:
|
||
_laser_uart.write(LASER_OFF)
|
||
_laser_uart.read(-1)
|
||
print("[EXIT] 激光已关闭")
|
||
except Exception:
|
||
pass
|
||
print("[EXIT] 测试结束")
|
||
if _key_fd:
|
||
_key_fd.close()
|
||
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""
|
||
激光中心点检测单元测试（单文件，无项目依赖）
|
||
直接使用 maix 标准库，实现红色激光点坐标检测
|
||
|
||
运行方式：
|
||
python3 test/test_laser_center_point.py
|
||
|
||
Ctrl+C 退出，按 s 保存截图
|
||
"""
|
||
|
||
from maix import camera, display, image, time, app, uart, pinmap
|
||
import os
|
||
import struct
|
||
import select
|
||
|
||
_USE_CV = False
|
||
try:
|
||
import cv2
|
||
import numpy as np
|
||
_USE_CV = True
|
||
except ImportError:
|
||
pass
|
||
|
||
WIDTH = 640
|
||
HEIGHT = 480
|
||
THRESHOLD = 120
|
||
RED_RATIO = 1.3
|
||
SEARCH_RADIUS = 60
|
||
|
||
|
||
def read_key_ev():
|
||
"""非阻塞读取 /dev/input/event0 按键（返回 key_code 或 -1）"""
|
||
try:
|
||
r, _, _ = select.select([_key_fd], [], [], 0)
|
||
if r:
|
||
event = _key_fd.read(16)
|
||
if len(event) == 16:
|
||
_, _, etype, code, value = struct.unpack("IIHHI", event)
|
||
if etype == 1 and value == 1:
|
||
return code
|
||
except Exception:
|
||
pass
|
||
return -1
|
||
|
||
|
||
def find_ellipse(img_cv, cx, cy, roi_r, th, ratio):
|
||
x1 = max(0, cx - roi_r)
|
||
x2 = min(WIDTH, cx + roi_r)
|
||
y1 = max(0, cy - roi_r)
|
||
y2 = min(HEIGHT, cy + roi_r)
|
||
roi = img_cv[y1:y2, x1:x2]
|
||
if roi.size == 0:
|
||
return None
|
||
r = roi[:, :, 0].astype(np.int32)
|
||
g = roi[:, :, 1].astype(np.int32)
|
||
b = roi[:, :, 2].astype(np.int32)
|
||
mask = (r > th) & (r > g * ratio) & (r > b * ratio)
|
||
oe = (r > 200) & (g > 200) & (b > 200) & (r >= g) & (r >= b) & ((r - g) > 10) & ((r - b) > 10)
|
||
combined = (mask | oe).astype(np.uint8) * 255
|
||
contours, _ = cv2.findContours(combined, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||
if not contours:
|
||
return None
|
||
largest = max(contours, key=cv2.contourArea)
|
||
if cv2.contourArea(largest) < 5:
|
||
return None
|
||
cnt = largest.copy()
|
||
for pt in cnt:
|
||
pt[0][0] += x1
|
||
pt[0][1] += y1
|
||
if len(cnt) >= 5:
|
||
(ex, ey), (ew, eh), ang = cv2.fitEllipse(cnt)
|
||
mask_ellipse = np.zeros((HEIGHT, WIDTH), dtype=np.uint8)
|
||
cv2.ellipse(mask_ellipse, (int(ex), int(ey)), (int(ew / 2), int(eh / 2)), ang, 0, 360, 255, -1)
|
||
brightness = img_cv[:, :, 0].astype(np.int32) + img_cv[:, :, 1].astype(np.int32) + img_cv[:, :, 2].astype(np.int32)
|
||
masked = np.where(mask_ellipse > 0, brightness, 0)
|
||
vals = masked[masked > 0]
|
||
if len(vals) > 0:
|
||
bth = np.percentile(vals, 90)
|
||
bmask = (masked >= bth).astype(np.uint8) * 255
|
||
bcontours, _ = cv2.findContours(bmask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||
if bcontours:
|
||
blargest = max(bcontours, key=cv2.contourArea)
|
||
if cv2.contourArea(blargest) >= 3 and len(blargest) >= 5:
|
||
(ix, iy), _, _ = cv2.fitEllipse(blargest)
|
||
return (float(ix), float(iy))
|
||
M = cv2.moments(blargest)
|
||
if M["m00"] > 0:
|
||
return (float(M["m10"] / M["m00"]), float(M["m01"] / M["m00"]))
|
||
return (float(ex), float(ey))
|
||
M = cv2.moments(cnt)
|
||
if M["m00"] > 0:
|
||
return (float(M["m10"] / M["m00"]), float(M["m01"] / M["m00"]))
|
||
return None
|
||
|
||
|
||
def find_brightest_bytes(frame, cx, cy, roi_r, th, ratio):
|
||
"""使用 frame.to_bytes() 两阶段搜索，避免 cv2 转换"""
|
||
x1 = max(0, cx - roi_r)
|
||
x2 = min(WIDTH, cx + roi_r)
|
||
y1 = max(0, cy - roi_r)
|
||
y2 = min(HEIGHT, cy + roi_r)
|
||
data = frame.to_bytes()
|
||
best_score = 0
|
||
best_pos = None
|
||
# 第一阶段：隔点粗搜
|
||
for y in range(y1, y2, 2):
|
||
for x in range(x1, x2, 2):
|
||
idx = (y * WIDTH + x) * 3
|
||
r = data[idx]; g = data[idx+1]; b = data[idx+2]
|
||
if (r > th and r > g * ratio and r > b * ratio) or \
|
||
(r > 200 and g > 200 and b > 200 and r >= g and r >= b and (r - g) > 10 and (r - b) > 10):
|
||
score = r + g + b
|
||
dx = x - cx; dy = y - cy
|
||
score *= max(0.5, 1.0 - ((dx*dx + dy*dy) ** 0.5 / roi_r) * 0.5)
|
||
if score > best_score:
|
||
best_score = score
|
||
best_pos = (x, y)
|
||
if best_pos is None:
|
||
return None
|
||
# 第二阶段：候选点 7x7 精细搜索
|
||
fx, fy = best_pos
|
||
x1f = max(0, fx - 3); x2f = min(WIDTH, fx + 4)
|
||
y1f = max(0, fy - 3); y2f = min(HEIGHT, fy + 4)
|
||
best_bright = 0
|
||
final_pos = best_pos
|
||
for y in range(y1f, y2f):
|
||
for x in range(x1f, x2f):
|
||
idx = (y * WIDTH + x) * 3
|
||
r = data[idx]; g = data[idx+1]; b = data[idx+2]
|
||
if (r > th and r > g * ratio and r > b * ratio) or \
|
||
(r > 200 and g > 200 and b > 200 and r >= g and r >= b and (r - g) > 10 and (r - b) > 10):
|
||
rgb_sum = r + g + b
|
||
if rgb_sum > best_bright:
|
||
best_bright = rgb_sum
|
||
final_pos = (float(x), float(y))
|
||
return final_pos
|
||
|
||
|
||
# 打开键盘输入设备
|
||
_key_fd = None
|
||
try:
|
||
_key_fd = open("/dev/input/event0", "rb")
|
||
except Exception:
|
||
try:
|
||
_key_fd = open("/dev/input/event1", "rb")
|
||
except Exception:
|
||
_key_fd = None
|
||
|
||
print("=" * 50)
|
||
print("激光中心点检测单元测试")
|
||
print("=" * 50)
|
||
print()
|
||
|
||
cam = camera.Camera(WIDTH, HEIGHT)
|
||
disp = display.Display()
|
||
print("[OK] 摄像头和显示初始化完成")
|
||
|
||
# 初始化激光串口
|
||
_laser_on = False
|
||
_laser_uart = None
|
||
try:
|
||
pinmap.set_pin_function("A18", "UART1_RX")
|
||
pinmap.set_pin_function("A19", "UART1_TX")
|
||
_laser_uart = uart.UART("/dev/ttyS1", 9600)
|
||
_laser_uart.read(-1)
|
||
print("[OK] 激光串口初始化完成")
|
||
except Exception as e:
|
||
print(f"[WARN] 激光串口初始化失败: {e}")
|
||
|
||
LASER_ON = bytes([0xAA, 0x00, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x01, 0xC1])
|
||
LASER_OFF = bytes([0xAA, 0x00, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x00, 0xC0])
|
||
|
||
# 默认开启激光
|
||
if _laser_uart:
|
||
try:
|
||
_laser_uart.write(LASER_ON)
|
||
time.sleep_ms(50)
|
||
_laser_uart.read(-1)
|
||
_laser_on = True
|
||
print("[OK] 激光已开启")
|
||
except Exception as e:
|
||
print(f"[WARN] 开启激光失败: {e}")
|
||
print()
|
||
|
||
pos_ellipse = None
|
||
pos_bright = None
|
||
frame_count = 0
|
||
use_ellipse = True
|
||
|
||
while not app.need_exit():
|
||
frame = cam.read()
|
||
if frame is None:
|
||
time.sleep_ms(10)
|
||
continue
|
||
|
||
frame_count += 1
|
||
|
||
cx, cy = WIDTH // 2, HEIGHT // 2
|
||
|
||
t0 = time.ticks_ms()
|
||
pos_bright = find_brightest_bytes(frame, cx, cy, SEARCH_RADIUS, THRESHOLD, RED_RATIO)
|
||
t1 = time.ticks_ms()
|
||
|
||
pos_ellipse = None
|
||
if _USE_CV:
|
||
img_cv = image.image2cv(frame, False, False)
|
||
t2 = time.ticks_ms()
|
||
pos_ellipse = find_ellipse(img_cv, cx, cy, SEARCH_RADIUS, THRESHOLD, RED_RATIO)
|
||
t3 = time.ticks_ms()
|
||
else:
|
||
img_cv = None
|
||
t3 = t2 = t1
|
||
|
||
dt_b = abs(time.ticks_diff(t0, t1))
|
||
dt_e = abs(time.ticks_diff(t2, t3))
|
||
|
||
if frame_count % 5 == 0:
|
||
e_str = f"({pos_ellipse[0]:.1f},{pos_ellipse[1]:.1f})" if pos_ellipse else "None"
|
||
b_str = f"({pos_bright[0]:.1f},{pos_bright[1]:.1f})" if pos_bright else "None"
|
||
print(f"[LASER] ellipse={e_str} ({dt_e}ms) brightest={b_str} ({dt_b}ms) "
|
||
f"th={THRESHOLD} ratio={RED_RATIO} radius={SEARCH_RADIUS}")
|
||
|
||
pos = pos_ellipse if use_ellipse else pos_bright
|
||
if img_cv is not None:
|
||
cv2.circle(img_cv, (cx, cy), SEARCH_RADIUS, (0, 255, 0), 1)
|
||
cv2.circle(img_cv, (cx, cy), 2, (0, 255, 0), -1)
|
||
if pos:
|
||
x, y = int(pos[0]), int(pos[1])
|
||
cv2.circle(img_cv, (x, y), 6, (0, 0, 255), 2)
|
||
cv2.line(img_cv, (x - 14, y), (x + 14, y), (0, 0, 255), 1)
|
||
cv2.line(img_cv, (x, y - 14), (x, y + 14), (0, 0, 255), 1)
|
||
cv2.putText(img_cv, f"({x},{y})", (x + 10, y - 10),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
|
||
info = [
|
||
f"pos={pos if pos else 'None'}",
|
||
f"method={'ellipse' if use_ellipse else 'brightest'} th={THRESHOLD} ratio={RED_RATIO}",
|
||
f"laser={'ON' if _laser_on else 'OFF'}",
|
||
]
|
||
for i, line in enumerate(info):
|
||
cv2.putText(img_cv, line, (8, 20 + i * 22),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1, cv2.LINE_AA)
|
||
display_frame = image.cv2image(img_cv, False, False)
|
||
else:
|
||
display_frame = frame
|
||
|
||
disp.show(display_frame)
|
||
|
||
# 按键处理（非阻塞）
|
||
key = read_key_ev()
|
||
if key > 0:
|
||
c = chr(key & 0xFF) if key < 256 else ""
|
||
if key == 113 or key == 81 or key == 0x1b: # q/Q/ESC
|
||
break
|
||
if c == "e" or key == 18: # e
|
||
use_ellipse = not use_ellipse
|
||
print(f"[KEY] Method: {'ellipse' if use_ellipse else 'brightest'}")
|
||
if c == "l" or key == 12: # l
|
||
_laser_on = not _laser_on
|
||
if _laser_uart:
|
||
try:
|
||
_laser_uart.write(LASER_ON if _laser_on else LASER_OFF)
|
||
time.sleep_ms(30)
|
||
_laser_uart.read(-1)
|
||
print(f"[KEY] Laser: {'ON' if _laser_on else 'OFF'}")
|
||
except Exception as e:
|
||
print(f"[KEY] Laser error: {e}")
|
||
else:
|
||
print("[KEY] Laser UART not available")
|
||
time.sleep_ms(30)
|
||
|
||
# 关闭激光
|
||
if _laser_on and _laser_uart:
|
||
try:
|
||
_laser_uart.write(LASER_OFF)
|
||
_laser_uart.read(-1)
|
||
print("[EXIT] 激光已关闭")
|
||
except Exception:
|
||
pass
|
||
print("[EXIT] 测试结束")
|
||
if _key_fd:
|
||
_key_fd.close()
|