not issue-0-viewpy（k230） #2
Description
import serial
import time
import cv2
import numpy as np
import math
import RPi.GPIO as GPIO
import signal
import sys
==============================================
配置参数（根据硬件实际情况修改）
==============================================
串口配置
SERIAL_PORT = "/dev/ttyS1" # 与TI MSPM0通信串口
SERVO_PORT = "/dev/ttyS2" # 舵机控制串口
SERIAL_BAUD = 115200 # 串口波特率
摄像头配置
CAMERA_INDEX = 0 # 摄像头索引
CAM_WIDTH = 1280 # 摄像头宽度
CAM_HEIGHT = 720 # 摄像头高度
激光与GPIO配置
LASER_PIN = 12 # 激光控制引脚
POWER_AIM_PIN = 13 # 瞄准模块电源检测引脚
POWER_CAR_PIN = 14 # 小车电源检测引脚
舵机参数
PAN_ID = 1 # 水平舵机ID
TILT_ID = 2 # 垂直舵机ID
SERVO_BAUD = 115200 # 舵机通信波特率
PAN_MIN, PAN_MAX = 45, 135 # 水平角度范围(度)
TILT_MIN, TILT_MAX = 60, 120 # 垂直角度范围(度)
靶心识别参数（红色靶心）
RED_LOW = np.array([0, 120, 70]) # HSV红色下限1
RED_HIGH = np.array([10, 255, 255]) # HSV红色上限1
RED_LOW2 = np.array([170, 120, 70]) # HSV红色下限2
RED_HIGH2 = np.array([180, 255, 255])# HSV红色上限2
TARGET_MIN_RADIUS = 0 # 靶心最小半径(像素)
TARGET_MAX_RADIUS = 5 # 靶心最大半径(像素)
场地参数
TRACK_LENGTH = 100 # 正方形轨迹边长(cm)
TARGET_DISTANCE = 50 # 靶距离AB线段距离(cm)
==============================================
核心工具类（迁移并优化）
==============================================
class RectTracker:
"""多帧矩形中心点跟踪器，过滤瞬时噪声"""
def init(self, window_size=3, max_deviation=5):
self.window = [] # 缓存最近N帧中心点
self.window_size = window_size # 滑动窗口大小
self.max_deviation = max_deviation # 最大允许偏差（像素）
def update(self, new_center):
"""更新窗口并返回平滑后的中心点（无效则返回None）"""
if new_center is None:
if self.window:
self.window.pop(0)
return None
self.window.append(new_center)
if len(self.window) > self.window_size:
self.window.pop(0)
# 窗口满且所有点偏差在阈值内时，返回平均值
if len(self.window) == self.window_size:
avg_x = sum(c[0] for c in self.window) / self.window_size
avg_y = sum(c[1] for c in self.window) / self.window_size
if (max(abs(c[0]-avg_x) for c in self.window) < self.max_deviation and
max(abs(c[1]-avg_y) for c in self.window) < self.max_deviation):
return (avg_x, avg_y) # 返回平滑后的中心
return None
def calculate_angle(p1, p2, p3):
"""计算三点构成的角度（用于判断矩形直角）"""
a2 = (p2[0] - p3[0])2 + (p2[1] - p3[1]) 2
b2 = (p1[0] - p3[0])2 + (p1[1] - p3[1]) 2
c2 = (p1[0] - p2[0])2 + (p1[1] - p2[1]) 2
if a2 == 0 or c2 == 0:
return 0
cos_angle = (a2 + c2 - b2) / (2 * math.sqrt(a2) * math.sqrt(c2))
cos_angle = max(-1.0, min(1.0, cos_angle)) # 避免数值溢出
return math.degrees(math.acos(cos_angle))
def refine_corners_subpixel(img, corners):
"""亚像素级角点优化（提高角点坐标精度）"""
gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
refined = []
for (x, y) in corners:
min_grad = float('inf')
best_x, best_y = x, y
# 5x5邻域搜索最优梯度点
for dx in [-2, -1, 0, 1, 2]:
for dy in [-2, -1, 0, 1, 2]:
nx, ny = int(x + dx), int(y + dy)
if 0 <= nx < gray_img.shape[1] and 0 <= ny < gray_img.shape[0]:
# 计算梯度（水平+垂直）
grad = abs(int(gray_img[ny, nx+1]) - int(gray_img[ny, nx-1])) +
abs(int(gray_img[ny+1, nx]) - int(gray_img[ny-1, nx]))
if grad < min_grad:
min_grad = grad
best_x, best_y = nx, ny
refined.append((best_x, best_y))
return refined
def calculate_calibrated_center(corners):
"""基于透视校正的A4靶面中心计算（物理尺寸210mm×297mm）"""
a4_phys = [(0, 0), (210, 0), (210, 297), (0, 297)] # 物理坐标(mm)
img_pts = corners[:4] # 确保4个角点
# 计算透视变换矩阵（使用OpenCV提高精度）
img_pts_np = np.float32(img_pts)
phys_pts_np = np.float32(a4_phys)
H, _ = cv2.findHomography(img_pts_np, phys_pts_np) # 透视变换矩阵
# 物理中心(105mm,148.5mm)转换为图像坐标
center_phys = np.float32([[105, 148.5]]).reshape(-1, 1, 2)
center_img = cv2.perspectiveTransform(center_phys, H) # 透视反投影
return (center_img[0][0][0], center_img[0][0][1]) # 图像中心(x,y)
def detect_target_rect(img):
"""检测A4靶面黑色边框矩形，返回角点（适配OpenCV）"""
# 转换为灰度图并二值化（黑色边框检测）
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
_, thresh = cv2.threshold(gray, 100, 255, cv2.THRESH_BINARY_INV) # 黑色区域为白
# 形态学操作去除噪声
kernel = np.ones((5, 5), np.uint8)
thresh = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)
# 寻找轮廓
contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
valid_rects = []
for cnt in contours:
# 轮廓近似为多边形
epsilon = 0.02 * cv2.arcLength(cnt, True)
approx = cv2.approxPolyDP(cnt, epsilon, True)
if len(approx) != 4: # 过滤非四边形
continue
# 提取角点（转换为(x,y)格式）
corners = [tuple(pt[0]) for pt in approx]
# 尺寸过滤（A4纸在图像中至少200x280像素，适配1280x720）
w = max(c[0] for c in corners) - min(c[0] for c in corners)
h = max(c[1] for c in corners) - min(c[1] for c in corners)
if w < 200 or h < 280:
continue
# 宽高比过滤（A4比例210:297≈0.707）
aspect_ratio = min(w/h, h/w)
if not (0.65 < aspect_ratio < 0.75):
continue
# 直角过滤（四个角接近90度）
angles = [calculate_angle(corners[i], corners[(i+1)%4], corners[(i+2)%4]) for i in range(4)]
if not all(abs(angle - 90) < 15 for angle in angles):
continue
# 亚像素优化角点
refined_corners = refine_corners_subpixel(img, corners)
valid_rects.append((refined_corners, w*h)) # 按面积排序
if valid_rects:
valid_rects.sort(key=lambda x: x[1], reverse=True)
return valid_rects[0][0] # 返回最大矩形的角点
return None
==============================================
舵机控制类
==============================================
class ServoController:
"""舵机控制器，负责精确控制舵机角度"""
def init(self, port):
self.ser = serial.Serial(port, SERVO_BAUD, timeout=0.1)
time.sleep(1) # 等待串口初始化
# 舵机角度补偿表(通过校准获得)
self.pan_compensation = self.load_compensation_data("pan_calibration.csv")
self.tilt_compensation = self.load_compensation_data("tilt_calibration.csv")
def load_compensation_data(self, filename):
"""加载舵机校准补偿数据"""
try:
data = np.genfromtxt(filename, delimiter=',', skip_header=1)
return {int(row[0]): row[1] for row in data}
except:
# 如果没有校准数据，返回空字典
return {}
def set_angle(self, servo_id, angle):
"""设置舵机角度，包含校准补偿"""
if not (0 <= angle <= 180):
return False
# 应用校准补偿
if servo_id == PAN_ID and angle in self.pan_compensation:
angle += self.pan_compensation[angle]
elif servo_id == TILT_ID and angle in self.tilt_compensation:
angle += self.tilt_compensation[angle]
# 确保角度在有效范围内
if servo_id == PAN_ID:
angle = max(PAN_MIN, min(PAN_MAX, angle))
else:
angle = max(TILT_MIN, min(TILT_MAX, angle))
# 计算脉冲宽度 (500-2500us对应0-180度)
pulse = 500 + int(angle * 2000 / 180)
# 构建控制帧
frame = [0xAA, servo_id, 0x01, 0x02, (pulse>>8)&0xFF, pulse&0xFF]
checksum = frame[0] ^ frame[1] ^ frame[2] ^ frame[3] ^ frame[4] ^ frame[5]
frame += [checksum, 0x55]
try:
self.ser.write(bytearray(frame))
return True
except Exception as e:
print(f"舵机控制错误: {e}")
return False
==============================================
瞄准系统主类
==============================================
class AimingSystem:
"""瞄准系统主类，协调所有功能模块"""
def init(self):
# 初始化硬件资源
self.init_gpio()
self.ti_ser = self.init_serial(SERIAL_PORT, SERIAL_BAUD)
self.servo = ServoController(SERVO_PORT)
self.cap = self.init_camera(CAMERA_INDEX)
# 系统状态
self.laser_on = False
self.running = True
self.car_position = (0, 0) # 小车位置(x, y)
self.current_segment = "AB" # 当前行驶路段
self.operation_mode = "idle" # 操作模式: idle, aiming, drawing, tracking
# 圈数与时间跟踪
self.lap_count = 0 # 当前圈数
self.last_segment = None # 上一路段，用于检测圈数递增
self.target_laps = 0 # 目标圈数
self.max_time = 0 # 最大允许时间
self.start_time = 0 # 任务开始时间
self.last_ti_time = time.time() # 最后一次收到TI指令的时间
# 时间同步
self.ti_time = 0
self.sync_offset = 0
# 目标检测相关
self.circle_history = [] # 红色圆检测历史
self.rect_tracker = RectTracker(window_size=3, max_deviation=5) # 矩形中心跟踪器
self.rect_detected = False # 矩形检测成功标记
# 校准系统
self.calibrate_servo()
# 注册信号处理器
signal.signal(signal.SIGINT, self.handle_shutdown)
signal.signal(signal.SIGTERM, self.handle_shutdown)
print("瞄准系统初始化完成")
def init_gpio(self):
"""初始化GPIO"""
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(LASER_PIN, GPIO.OUT)
GPIO.setup(POWER_AIM_PIN, GPIO.IN) # 瞄准模块电源状态
GPIO.setup(POWER_CAR_PIN, GPIO.IN) # 小车电源状态
def init_serial(self, port, baudrate):
"""初始化串口通信"""
try:
ser = serial.Serial(port, baudrate, timeout=0.1)
time.sleep(1)
return ser
except Exception as e:
print(f"串口初始化失败 {port}: {e}")
return None
def init_camera(self, index):
"""初始化摄像头"""
cap = cv2.VideoCapture(index)
if not cap.isOpened():
print("摄像头初始化失败")
return None
# 设置摄像头分辨率
cap.set(cv2.CAP_PROP_FRAME_WIDTH, CAM_WIDTH)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, CAM_HEIGHT)
return cap
def calibrate_servo(self):
"""校准舵机到中心位置"""
print("开始舵机校准...")
self.servo.set_angle(PAN_ID, 90)
self.servo.set_angle(TILT_ID, 90)
time.sleep(1)
print("舵机校准完成")
def select_best_circle(self, circles):
"""选择最可能是靶心的圆"""
h, w = CAM_HEIGHT, CAM_WIDTH
center = (w/2, h/2)
best_circle = None
min_distance = float('inf')
for circle in circles[0, :]:
cx, cy, r = circle
distance = np.sqrt((cx - center[0])**2 + (cy - center[1])** 2)
# 优先选择接近中心且半径符合要求的圆
if distance < min_distance and TARGET_MIN_RADIUS <= r <= TARGET_MAX_RADIUS:
min_distance = distance
best_circle = circle
return best_circle
def detect_target(self):
"""改进的靶心识别算法：优先矩形中心，失败则用红色圆"""
if not self.cap or not self.cap.isOpened():
return None
ret, frame = self.cap.read()
if not ret:
return None
# 步骤1：检测矩形并计算中心（优先策略）
corners = detect_target_rect(frame)
if corners:
try:
# 计算透视校正后的中心
rect_center = calculate_calibrated_center(corners)
# 多帧平滑
smoothed_center = self.rect_tracker.update(rect_center)
if smoothed_center:
self.rect_detected = True
# 调试：绘制矩形角点和中心
for (x, y) in corners:
cv2.circle(frame, (int(x), int(y)), 5, (0, 255, 0), -1) # 角点标记
cv2.circle(frame, (int(smoothed_center[0]), int(smoothed_center[1])), 8, (0, 0, 255), -1) # 中心标记
h, w = frame.shape[:2]
# 归一化到-1~1范围
x_norm = (smoothed_center[0] - w/2) / (w/2)
y_norm = (smoothed_center[1] - h/2) / (h/2)
cv2.imshow("Target Detection", frame)
cv2.waitKey(1)
return (x_norm, y_norm)
except Exception as e:
print(f"矩形中心计算失败: {e}")
self.rect_detected = False
# 步骤2：矩形检测失败，fallback到红色圆检测
self.rect_detected = False
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
mask1 = cv2.inRange(hsv, RED_LOW, RED_HIGH)
mask2 = cv2.inRange(hsv, RED_LOW2, RED_HIGH2)
mask = mask1 | mask2
# 形态学处理去除噪声
kernel = np.ones((3, 3), np.uint8)
mask = cv2.erode(mask, kernel, iterations=1)
mask = cv2.dilate(mask, kernel, iterations=2)
# 转换为灰度图用于霍夫圆检测
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
gray = cv2.bitwise_and(gray, gray, mask=mask)
# 霍夫圆检测 - 专门检测小半径的靶心
circles = cv2.HoughCircles(
gray,
method=cv2.HOUGH_GRADIENT,
dp=1,
minDist=20,
param1=50,
param2=10,
minRadius=TARGET_MIN_RADIUS,
maxRadius=TARGET_MAX_RADIUS
)
if circles is not None:
circles = np.uint16(np.around(circles))
current_circle = self.select_best_circle(circles)
if current_circle is not None:
# 记录历史检测结果
self.circle_history.append(current_circle)
# 连续3帧稳定检测才确认靶心
if len(self.circle_history) >= 3:
# 计算平均值减少抖动
avg_cx = np.mean([c[0] for c in self.circle_history[-3:]])
avg_cy = np.mean([c[1] for c in self.circle_history[-3:]])
# 调试：绘制靶心
cv2.circle(frame, (int(avg_cx), int(avg_cy)), 5, (0, 0, 255), -1)
cv2.imshow("Target Detection", frame)
cv2.waitKey(1)
# 清空历史记录，准备下一次检测
self.circle_history = []
h, w = frame.shape[:2]
# 归一化到-1~1范围
x_norm = (avg_cx - w/2) / (w/2)
y_norm = (avg_cy - h/2) / (h/2)
return (x_norm, y_norm)
# 如果没有足够的稳定检测，逐步清空历史
if len(self.circle_history) > 0:
self.circle_history.pop(0)
# 显示空帧（调试用）
cv2.imshow("Target Detection", frame)
cv2.waitKey(1)
return None
def calculate_compensated_angles(self, x_norm, y_norm):
"""根据小车位置计算补偿后的舵机角度，包含动态焦距补偿"""
# 基础角度计算（矩形检测成功时降低灵敏度，减少抖动）
if self.rect_detected:
pan = 90 - int(x_norm * 30) # 系数从45→30（更平缓）
tilt = 90 - int(y_norm * 20) # 系数从30→20
else:
pan = 90 - int(x_norm * 45) # 原系数（圆检测时更灵敏）
tilt = 90 - int(y_norm * 30)
# 根据小车位置进行补偿
x, y = self.car_position
# 不同路段的补偿因子
if self.current_segment == "AB": # 从A到B行驶
pan -= 2 # 基础补偿
elif self.current_segment == "BC": # 从B到C行驶
pan_comp = self.map_range(y, 0, TRACK_LENGTH, -2, 2)
pan += pan_comp
tilt -= 1
elif self.current_segment == "CD": # 从C到D行驶
pan += 2
elif self.current_segment == "DA": # 从D到A行驶
pan_comp = self.map_range(y, TRACK_LENGTH, 0, 2, -2)
pan += pan_comp
tilt += 1
# 动态焦距补偿（根据距离调整）
if self.current_segment in ["BC", "DA"]:
# 计算小车到靶面的实际距离
distance = math.sqrt((x - 50)**2 + (y - TRACK_LENGTH/2)** 2)
# 相对于基准距离(50cm)的比例
distance_ratio = distance / 50
# 距离越远，补偿越大
pan *= distance_ratio
tilt *= distance_ratio
return pan, tilt
def aim_target(self, timeout=2):
"""在指定时间内瞄准靶心"""
self.operation_mode = "aiming"
self.turn_laser(True)
start_time = time.time()
success = False
while time.time() - start_time < timeout and self.running:
target = self.detect_target()
if target:
x_norm, y_norm = target
pan, tilt = self.calculate_compensated_angles(x_norm, y_norm)
# 设置舵机角度
self.servo.set_angle(PAN_ID, pan)
self.servo.set_angle(TILT_ID, tilt)
# 检查是否瞄准成功（偏差小于阈值）
if abs(x_norm) < 0.03 and abs(y_norm) < 0.03:
success = True
break
time.sleep(0.03) # 控制频率~33Hz
# 发送瞄准结果给TI
if self.ti_ser:
result = "SUCCESS" if success else "FAIL"
self.ti_ser.write(f"AIM_RESULT:{result}\n".encode())
self.operation_mode = "idle"
return success
def draw_circle(self):
"""同步画圆：小车行驶1圈，激光画1圈，使用位置闭环同步"""
self.operation_mode = "drawing"
self.turn_laser(True)
initial_lap = self.lap_count # 记录开始时的圈数
while self.running and self.lap_count < initial_lap + self.target_laps:
# 从当前位置计算沿轨迹的累计行驶距离
x, y = self.car_position
if self.current_segment == "AB":
segment_dist = y
elif self.current_segment == "BC":
segment_dist = TRACK_LENGTH + x
elif self.current_segment == "CD":
segment_dist = 2 * TRACK_LENGTH + (TRACK_LENGTH - y)
else: # DA段
segment_dist = 3 * TRACK_LENGTH + (TRACK_LENGTH - x)
# 计算总行驶距离（包含已完成的圈数）
total_completed = (self.lap_count - initial_lap) * 4 * TRACK_LENGTH + segment_dist
total_target = self.target_laps * 4 * TRACK_LENGTH # 目标总距离
if total_target == 0:
break
# 行驶距离映射到画圆角度（1圈轨迹=1圈圆周）
angle = 2 * math.pi * (total_completed / total_target)
# 半径6cm的圆（转换为角度偏移）
x_offset = math.cos(angle) * 6
y_offset = math.sin(angle) * 6
# 基础舵机角度
pan = 90 + int(x_offset * 5)
tilt = 90 + int(y_offset * 5)
# 获取当前实际光斑位置用于误差补偿
target = self.detect_target()
if target:
x_norm, y_norm = target
# 计算实际角度
actual_angle = math.atan2(y_norm, x_norm)
# 计算角度误差
angle_error = angle - actual_angle
# 应用误差补偿（比例控制）
pan += int(angle_error * 180/math.pi * 0.5)
# 确保角度在有效范围内
pan = max(PAN_MIN, min(PAN_MAX, pan))
tilt = max(TILT_MIN, min(TILT_MAX, tilt))
# 设置舵机角度
self.servo.set_angle(PAN_ID, pan)
self.servo.set_angle(TILT_ID, tilt)
time.sleep(0.02) # 控制频率~50Hz
self.turn_laser(False)
self.operation_mode = "idle"
if self.ti_ser:
self.ti_ser.write("DRAW:DONE\n".encode())
def turn_laser(self, on):
"""控制激光开关"""
# 检查瞄准模块电源是否开启
if not GPIO.input(POWER_AIM_PIN):
print("瞄准模块电源未开启，无法控制激光")
return
self.laser_on = on
GPIO.output(LASER_PIN, on)
print(f"激光{'开启' if on else '关闭'}")
def sync_time(self, ti_timestamp):
"""与TI时间同步"""
k230_time = int(time.time() * 1000)
self.sync_offset = ti_timestamp - k230_time
self.ti_time = ti_timestamp
def update_car_position(self, x, y):
"""更新小车位置并判断当前路段，同时检测圈数"""
self.car_position = (x, y)
# 判断当前行驶路段
if x < 5 and 0 <= y <= TRACK_LENGTH:
self.current_segment = "AB"
elif y > TRACK_LENGTH - 5 and 0 <= x <= TRACK_LENGTH:
self.current_segment = "BC"
elif x > TRACK_LENGTH - 5 and 0 <= y <= TRACK_LENGTH:
self.current_segment = "CD"
elif y < 5 and 0 <= x <= TRACK_LENGTH:
self.current_segment = "DA"
# 检测圈数递增（从DA回到AB即为完成一圈）
if self.current_segment == "AB" and self.last_segment == "DA":
self.lap_count += 1
print(f"完成第{self.lap_count}圈")
self.last_segment = self.current_segment
def process_ti_commands(self):
"""处理来自TI MSPM0的指令，包含超时处理"""
if not self.ti_ser or not self.ti_ser.in_waiting:
# 超时判断（3秒无指令则停止）
if time.time() - self.last_ti_time > 3:
print("TI通信超时，停止激光")
self.turn_laser(False)
self.operation_mode = "idle"
return
# 更新最后通信时间
self.last_ti_time = time.time()
try:
cmd = self.ti_ser.readline().decode().strip()
if not cmd:
return
print(f"收到TI指令：{cmd}")
# 处理位置信息
if cmd.startswith("POS:"):
parts = cmd.split(",")
if len(parts) >= 3 and "TIME:" in parts[2]:
x = int(parts[0].replace("POS:", ""))
y = int(parts[1])
time_str = parts[2].split("TIME:")[1]
self.update_car_position(x, y)
self.sync_time(int(time_str))
# 处理瞄准指令
elif cmd == "AIM2":
self.aim_target(timeout=2)
elif cmd == "AIM4":
self.aim_target(timeout=4)
# 处理画圆指令
elif cmd == "DRAW":
self.draw_circle()
# 处理开始行驶指令（设置目标圈数和最大时间）
elif cmd.startswith("START:"):
# 格式: START:<圈数>,<最大时间>
parts = cmd.split(":")[1].split(",")
if len(parts) == 2:
self.target_laps = int(parts[0])
self.max_time = int(parts[1])
self.start_time = time.time()
self.lap_count = 0 # 重置圈数计数
print(f"开始任务：{self.target_laps}圈，最长{self.max_time}秒")
# 处理停止指令
elif cmd == "STOP":
self.turn_laser(False)
self.operation_mode = "idle"
self.target_laps = 0
self.max_time = 0
except Exception as e:
print(f"处理TI指令错误: {e}")
def handle_shutdown(self, signum, frame):
"""优雅关闭系统"""
print("\n收到关闭信号，正在关闭系统...")
self.running = False
def run(self):
"""系统主循环，包含行驶终止条件判断和电源保护"""
print("系统启动，等待指令...")
try:
while self.running:
# 检查电源状态
aiming_power = GPIO.input(POWER_AIM_PIN)
car_power = GPIO.input(POWER_CAR_PIN)
# 电源异常保护
if not car_power: # 小车电源断开
print("小车电源断开，紧急停止")
if self.ti_ser:
self.ti_ser.write("EMERGENCY_STOP\n".encode())
self.running = False
break
# 如果瞄准模块电源关闭，关闭激光
if not aiming_power and self.laser_on:
self.turn_laser(False)
# 处理TI指令
self.process_ti_commands()
# 检查行驶终止条件
if self.target_laps > 0:
# 检查超时
if time.time() - self.start_time > self.max_time:
print(f"超时({self.max_time}秒)，发送停止指令")
if self.ti_ser:
self.ti_ser.write("STOP\n".encode())
self.target_laps = 0
self.max_time = 0
# 检查圈数达标
if self.lap_count >= self.target_laps:
print(f"已完成{self.target_laps}圈，发送停止指令")
if self.ti_ser:
self.ti_ser.write("STOP\n".encode())
self.target_laps = 0
self.max_time = 0
time.sleep(0.05)
finally:
self.cleanup()
def cleanup(self):
"""释放资源"""
print("正在清理资源...")
self.turn_laser(False)
if self.cap:
self.cap.release()
cv2.destroyAllWindows() # 关闭调试窗口
if self.ti_ser and self.ti_ser.is_open:
self.ti_ser.close()
if hasattr(self.servo, 'ser') and self.servo.ser.is_open:
self.servo.ser.close()
GPIO.cleanup()
print("系统已关闭")
@staticmethod
def map_range(x, in_min, in_max, out_min, out_max):
"""将值从一个范围映射到另一个范围"""
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
==============================================
主程序入口
==============================================
if name == "main":
system = AimingSystem()
system.run()