帮我应用到这个代码上
import cv2
import numpy as np
import time
import math
import RPi.GPIO as GPIO
# 简化的激光点检测函数 - 亮点检测
def detect_bright_spot(gray_frame, min_brightness=220, min_area=5, min_circularity=0.7):
"""在灰度图像中检测亮点"""
# 阈值处理提取亮区
_, bright_mask = cv2.threshold(gray_frame, min_brightness, 255, cv2.THRESH_BINARY)
# 形态学操作去噪
kernel = np.ones((3, 3), np.uint8)
bright_mask = cv2.morphologyEx(bright_mask, cv2.MORPH_OPEN, kernel)
bright_mask = cv2.morphologyEx(bright_mask, cv2.MORPH_CLOSE, kernel)
# 寻找轮廓
contours, _ = cv2.findContours(bright_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if not contours:
return None, bright_mask
# 过滤轮廓
candidate_points = []
for contour in contours:
area = cv2.contourArea(contour)
if area < min_area:
continue
perimeter = cv2.arcLength(contour, True)
if perimeter == 0:
continue
circularity = 4 * np.pi * area / (perimeter * perimeter)
if circularity < min_circularity:
continue
M = cv2.moments(contour)
if M["m00"] == 0:
continue
cx = int(M["m10"] / M["m00"])
cy = int(M["m01"] / M["m00"])
candidate_points.append((cx, cy, area, circularity))
if not candidate_points:
return None, bright_mask
# 选择最亮的点（面积最大）
candidate_points.sort(key=lambda x: x[2], reverse=True)
best_point = candidate_points[0]
return (best_point[0], best_point[1]), bright_mask
# 视觉检测函数（保持原有逻辑）
def find_dominant_lines(edges, frame):
"""检测并连接线段，形成完整的条带边缘"""
min_line_length = cv2.getTrackbarPos("Min Line", "Parameters")
max_line_gap = cv2.getTrackbarPos("Max Gap", "Parameters")
lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=50,
minLineLength=min_line_length,
maxLineGap=max_line_gap)
upper_edge = None
lower_edge = None
upper_center_x = None
lower_center_x = None
debug_frame = frame.copy()
if lines is None:
return (upper_edge, upper_center_x), (lower_edge, lower_center_x), debug_frame
horizontal_lines = []
for line in lines:
x1, y1, x2, y2 = line[0]
angle = np.arctan2(y2 - y1, x2 - x1) * 180 / np.pi
if abs(angle) < 30 or abs(angle) > 150:
center_x = (x1 + x2) // 2
center_y = (y1 + y2) // 2
length = np.sqrt((x2 - x1)**2 + (y2 - y1)**2)
merged = False
for i, (cx, cy, count, min_y, max_y, total_x, total_y, total_length) in enumerate(horizontal_lines):
if abs(center_y - cy) < 15:
new_count = count + 1
new_min_y = min(min_y, min(y1, y2))
new_max_y = max(max_y, max(y1, y2))
new_total_x = total_x + center_x
new_total_y = total_y + center_y
new_total_length = total_length + length
new_cx = new_total_x / new_count
new_cy = new_total_y / new_count
horizontal_lines[i] = (new_cx, new_cy, new_count, new_min_y, new_max_y,
new_total_x, new_total_y, new_total_length)
merged = True
break
if not merged:
horizontal_lines.append((center_x, center_y, 1, min(y1, y2), max(y1, y2),
center_x, center_y, length))
if len(horizontal_lines) < 2:
return (upper_edge, upper_center_x), (lower_edge, lower_center_x), debug_frame
for cx, cy, count, min_y, max_y, _, _, _ in horizontal_lines:
avg_length = 300
x1 = int(cx - avg_length / 2)
x2 = int(cx + avg_length / 2)
cv2.line(debug_frame, (x1, int(cy)), (x2, int(cy)), (0, 255, 0), 2)
horizontal_lines.sort(key=lambda x: x[1])
candidate_edges = []
for i in range(len(horizontal_lines)):
cx, cy, count, min_y, max_y, _, _, length = horizontal_lines[i]
weight = count * length
candidate_edges.append((cy, min_y, max_y, weight, cx))
candidate_edges.sort(key=lambda x: x[3], reverse=True)
if len(candidate_edges) >= 2:
upper_edge = candidate_edges[0][0]
upper_center_x = candidate_edges[0][4]
lower_edge = candidate_edges[1][0]
lower_center_x = candidate_edges[1][4]
if upper_edge > lower_edge:
upper_edge, lower_edge = lower_edge, upper_edge
upper_center_x, lower_center_x = lower_center_x, upper_center_x
cv2.line(debug_frame, (0, int(upper_edge)), (frame.shape[1], int(upper_edge)), (0, 0, 255), 2)
cv2.circle(debug_frame, (int(upper_center_x), int(upper_edge)), 8, (255, 255, 0), -1)
cv2.line(debug_frame, (0, int(lower_edge)), (frame.shape[1], int(lower_edge)), (0, 0, 255), 2)
cv2.circle(debug_frame, (int(lower_center_x), int(lower_edge)), 8, (255, 255, 0), -1)
return (upper_edge, upper_center_x), (lower_edge, lower_center_x), debug_frame
# 计算偏转角度函数（保持原有逻辑）
def calculate_angles(offset_x, offset_y, frame_width, frame_height, hfov=70.0, vfov=50.0):
"""计算目标相对于视野中心的偏转角度"""
deg_per_pixel_x = hfov / frame_width
deg_per_pixel_y = vfov / frame_height
angle_x = offset_x * deg_per_pixel_x
angle_y = offset_y * deg_per_pixel_y
return angle_x, angle_y
# PID控制器类（保持原有逻辑）
class PIDController:
def __init__(self, kp=0.5, ki=0.01, kd=0.1, max_output=45):
self.kp = kp
self.ki = ki
self.kd = kd
self.max_output = max_output
self.previous_error = 0
self.integral = 0
self.last_time = time.time()
def update(self, error):
current_time = time.time()
dt = current_time - self.last_time
self.last_time = current_time
if dt <= 0:
return 0
proportional = self.kp * error
self.integral += error * dt
integral = self.ki * self.integral
derivative = self.kd * (error - self.previous_error) / dt
self.previous_error = error
output = proportional + integral + derivative
output = max(-self.max_output, min(self.max_output, output))
return output
def reset(self):
self.previous_error = 0
self.integral = 0
self.last_time = time.time()
# 舵机控制类（保持原有逻辑）
class ServoController:
def __init__(self, min_angle_x=-30, max_angle_x=50, min_angle_y=-10, max_angle_y=10,
min_pwm=500, max_pwm=2500, center_pwm_x=1500, center_pwm_y=1500):
# X方向参数
self.min_angle_x = min_angle_x
self.max_angle_x = max_angle_x
self.center_pwm_x = center_pwm_x
# Y方向参数
self.min_angle_y = min_angle_y
self.max_angle_y = max_angle_y
self.center_pwm_y = center_pwm_y
# 通用PWM参数
self.min_pwm = min_pwm
self.max_pwm = max_pwm
self.current_angle_x = 0
self.current_angle_y = 0
self.angle_limit = 0.5
self.pid_x = PIDController(kp=0.6, ki=0.02, kd=0.15, max_output=40)
self.pid_y = PIDController(kp=0.6, ki=0.02, kd=0.15, max_output=40)
def update(self, angle_x, angle_y):
if abs(angle_x) < self.angle_limit:
angle_x = 0
if abs(angle_y) < self.angle_limit:
angle_y = 0
adjustment_x = self.pid_x.update(angle_x)
adjustment_y = self.pid_y.update(angle_y)
self.current_angle_x = self._clamp_angle_x(self.current_angle_x + adjustment_x)
self.current_angle_y = self._clamp_angle_y(self.current_angle_y + adjustment_y)
pwm_x = self._angle_to_pwm_x(self.current_angle_x)
pwm_y = self._angle_to_pwm_y(self.current_angle_y)
return pwm_x, pwm_y
def _clamp_angle_x(self, angle):
return max(self.min_angle_x, min(self.max_angle_x, angle))
def _clamp_angle_y(self, angle):
return max(self.min_angle_y, min(self.max_angle_y, angle))
def _angle_to_pwm_x(self, angle):
if angle >= 0:
return int(self.center_pwm_x + (angle / self.max_angle_x) * (self.max_pwm - self.center_pwm_x))
else:
return int(self.center_pwm_x + (angle / self.min_angle_x) * (self.min_pwm - self.center_pwm_x))
def _angle_to_pwm_y(self, angle):
if angle >= 0:
return int(self.center_pwm_y + (angle / self.max_angle_y) * (self.max_pwm - self.center_pwm_y))
else:
return int(self.center_pwm_y + (angle / self.min_angle_y) * (self.min_pwm - self.center_pwm_y))
def reset(self):
self.current_angle_x = 0
self.current_angle_y = 0
self.pid_x.reset()
self.pid_y.reset()
return self.center_pwm_x, self.center_pwm_y
def draw_servo_indicator(frame, angle_x, angle_y):
"""在图像上绘制舵机角度指示器"""
height, width = frame.shape[:2]
cv2.putText(frame, f"X: {70 + angle_x:.1f}°", (10, height - 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1)
cv2.putText(frame, f"Y: {90 + angle_y:.1f}°", (10, height - 40),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1)
bar_width = 100
bar_height = 10
bar_x = width // 2 - bar_width // 2
bar_y = height - 30
cv2.rectangle(frame, (bar_x, bar_y), (bar_x + bar_width, bar_y + bar_height), (100, 100, 100), 1)
center_x = bar_x + bar_width // 2
position_x = center_x + int((angle_x / 50) * (bar_width // 2))
cv2.line(frame, (center_x, bar_y - 5), (center_x, bar_y + bar_height + 5), (0, 255, 0), 1)
cv2.circle(frame, (position_x, bar_y + bar_height // 2), 5, (0, 255, 255), -1)
bar_y = height - 60
cv2.rectangle(frame, (bar_x, bar_y), (bar_x + bar_width, bar_y + bar_height), (100, 100, 100), 1)
position_y = center_x + int((angle_y / 10) * (bar_width // 2))
cv2.line(frame, (center_x, bar_y - 5), (center_x, bar_y + bar_height + 5), (0, 255, 0), 1)
cv2.circle(frame, (position_y, bar_y + bar_height // 2), 5, (0, 255, 255), -1)
def setup_gpio(servo_x_pin, servo_y_pin):
GPIO.setmode(GPIO.BCM)
GPIO.setup(servo_x_pin, GPIO.OUT)
GPIO.setup(servo_y_pin, GPIO.OUT)
pwm_x = GPIO.PWM(servo_x_pin, 50)
pwm_y = GPIO.PWM(servo_y_pin, 50)
pwm_x.start(0)
pwm_y.start(0)
return pwm_x, pwm_y
def apply_servo_control(pwm_x_obj, pwm_y_obj, pwm_x, pwm_y):
duty_cycle_x = (pwm_x / 20000.0) * 100
duty_cycle_y = (pwm_y / 20000.0) * 100
pwm_x_obj.ChangeDutyCycle(duty_cycle_x)
pwm_y_obj.ChangeDutyCycle(duty_cycle_y)
def cleanup_gpio(pwm_x, pwm_y):
pwm_x.stop()
pwm_y.stop()
GPIO.cleanup()
def main():
SERVO_X_PIN = 15
SERVO_Y_PIN = 18
pwm_x_obj, pwm_y_obj = setup_gpio(SERVO_X_PIN, SERVO_Y_PIN)
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
cap.set(cv2.CAP_PROP_FPS, 60)
# 创建主参数窗口
cv2.namedWindow("Parameters")
cv2.createTrackbar("Brightness", "Parameters", 60, 100, lambda x: None)
cv2.createTrackbar("Contrast", "Parameters", 60, 100, lambda x: None)
cv2.createTrackbar("Canny1", "Parameters", 50, 200, lambda x: None)
cv2.createTrackbar("Canny2", "Parameters", 150, 300, lambda x: None)
cv2.createTrackbar("Min Line", "Parameters", 50, 300, lambda x: None)
cv2.createTrackbar("Max Gap", "Parameters", 20, 100, lambda x: None)
# 激光检测参数
cv2.createTrackbar("Laser Thresh", "Parameters", 220, 255, lambda x: None)
cv2.createTrackbar("Min Area", "Parameters", 5, 100, lambda x: None)
last_offset_x = 0
last_offset_y = 0
frame_count = 0
start_time = time.time()
fps = 0
HORIZONTAL_FOV = 70.0
VERTICAL_FOV = 50.0
# 初始化舵机控制器
servo_controller = ServoController(
min_angle_x=-30,
max_angle_x=50,
min_angle_y=-10,
max_angle_y=10,
center_pwm_x=1500,
center_pwm_y=1500
)
servo_enabled = True
tracking_history = []
max_history_length = 100
apply_servo_control(pwm_x_obj, pwm_y_obj, 1500, 1500)
time.sleep(0.5)
laser_in_zone = False
laser_start_time = 0
laser_duration = 0
detection_zone = None
last_detection_zone = None
while True:
ret, frame = cap.read()
if not ret:
print("摄像头读取失败，请检查连接")
break
frame = cv2.rotate(frame, cv2.ROTATE_180)
brightness = cv2.getTrackbarPos("Brightness", "Parameters")
contrast = cv2.getTrackbarPos("Contrast", "Parameters")
canny1 = cv2.getTrackbarPos("Canny1", "Parameters")
canny2 = cv2.getTrackbarPos("Canny2", "Parameters")
alpha = contrast / 50.0
beta = brightness - 50
frame = cv2.convertScaleAbs(frame, alpha=alpha, beta=beta)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
blurred = cv2.GaussianBlur(gray, (3, 3), 0)
edges = cv2.Canny(blurred, canny1, canny2)
(upper_edge, upper_center_x), (lower_edge, lower_center_x), debug_frame = find_dominant_lines(edges, frame.copy())
target_detected = False
if (upper_edge is not None and lower_edge is not None and
upper_center_x is not None and lower_center_x is not None and
upper_edge < lower_edge):
target_detected = True
white_center_y = (upper_edge + lower_edge) // 2
white_center_x = (upper_center_x + lower_center_x) // 2
frame_center_x = frame.shape[1] // 2
frame_center_y = frame.shape[0] // 2
offset_x_px = white_center_x - frame_center_x
offset_y_px = white_center_y - frame_center_y
filtered_offset_x = 0.7 * last_offset_x + 0.3 * offset_x_px
filtered_offset_y = 0.7 * last_offset_y + 0.3 * offset_y_px
last_offset_x = filtered_offset_x
last_offset_y = filtered_offset_y
angle_x, angle_y = calculate_angles(
filtered_offset_x,
filtered_offset_y,
frame.shape[1],
frame.shape[0],
HORIZONTAL_FOV,
VERTICAL_FOV
)
if servo_enabled:
pwm_x, pwm_y = servo_controller.update(angle_x, angle_y)
apply_servo_control(pwm_x_obj, pwm_y_obj, pwm_x, pwm_y)
else:
pwm_x, pwm_y = servo_controller.center_pwm_x, servo_controller.center_pwm_y
tracking_error = math.sqrt(offset_x_px**2 + offset_y_px**2)
tracking_history.append(tracking_error)
if len(tracking_history) > max_history_length:
tracking_history.pop(0)
cv2.circle(debug_frame, (frame_center_x, frame_center_y), 10, (0, 255, 0), 2)
cv2.circle(debug_frame, (int(white_center_x), int(white_center_y)), 10, (0, 0, 255), 2)
cv2.line(debug_frame,
(frame_center_x, frame_center_y),
(int(white_center_x), int(white_center_y)),
(255, 0, 0), 2)
# 绘制白色条带区域
cv2.rectangle(debug_frame,
(0, int(upper_edge)),
(frame.shape[1], int(lower_edge)),
(200, 200, 200), 1)
detection_zone = (0, int(upper_edge), frame.shape[1], int(lower_edge))
last_detection_zone = detection_zone
status_text = [
f"X偏移: {filtered_offset_x:.1f}px",
f"Y偏移: {filtered_offset_y:.1f}px",
f"X角度: {angle_x:.2f}°",
f"Y角度: {angle_y:.2f}°",
f"舵机角度 X: {70 + servo_controller.current_angle_x:.1f}°",
f"舵机角度 Y: {90 + servo_controller.current_angle_y:.1f}°",
f"舵机PWM X: {pwm_x}μs",
f"舵机PWM Y: {pwm_y}μs",
f"跟踪误差: {tracking_error:.1f}px",
f"平均误差: {np.mean(tracking_history):.1f}px",
f"FPS: {fps:.1f}",
f"舵机控制: {'启用' if servo_enabled else '禁用'}"
]
for i, text in enumerate(status_text):
cv2.putText(debug_frame, text, (10, 20 + i*25),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
else:
target_detected = False
if servo_enabled:
pwm_x, pwm_y = servo_controller.reset()
apply_servo_control(pwm_x_obj, pwm_y_obj, pwm_x, pwm_y)
else:
pwm_x, pwm_y = servo_controller.center_pwm_x, servo_controller.center_pwm_y
tracking_history = []
cv2.putText(debug_frame, "未检测到目标", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
if last_detection_zone is not None:
detection_zone = last_detection_zone
# 绘制上一次的检测区域
cv2.rectangle(debug_frame,
(0, detection_zone[1]),
(frame.shape[1], detection_zone[3]),
(100, 100, 100), 1)
# 使用简化的亮点检测方法
min_brightness = cv2.getTrackbarPos("Laser Thresh", "Parameters")
min_area = cv2.getTrackbarPos("Min Area", "Parameters")
laser_point, laser_mask = detect_bright_spot(gray, min_brightness, min_area)
if laser_point is not None:
lx, ly = laser_point
cv2.circle(debug_frame, (lx, ly), 10, (0, 0, 255), -1)
cv2.putText(debug_frame, "LASER", (lx + 15, ly),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
# 检查激光点是否在检测区域内
if detection_zone is not None:
_, zone_y1, _, zone_y2 = detection_zone
if zone_y1 <= ly <= zone_y2:
if not laser_in_zone:
laser_in_zone = True
laser_start_time = time.time()
print("激光点进入识别区域，开始计时")
else:
laser_duration = time.time() - laser_start_time
else:
if laser_in_zone:
print(f"激光点离开识别区域，停留时间: {laser_duration:.2f}秒")
laser_in_zone = False
laser_duration = 0
else:
cv2.putText(debug_frame, "无检测区域", (lx + 15, ly + 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
else:
if laser_in_zone:
print(f"激光点消失，停留时间: {laser_duration:.2f}秒")
laser_in_zone = False
laser_duration = 0
if laser_in_zone:
cv2.putText(debug_frame, f"计时: {laser_duration:.2f}秒",
(frame.shape[1] - 200, 60),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
frame_count += 1
if frame_count >= 10:
end_time = time.time()
fps = frame_count / (end_time - start_time)
frame_count = 0
start_time = time.time()
cv2.putText(debug_frame, f"FPS: {fps:.1f}", (debug_frame.shape[1] - 120, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
draw_servo_indicator(debug_frame, servo_controller.current_angle_x, servo_controller.current_angle_y)
cv2.imshow("Visual Servo Tracking", debug_frame)
cv2.imshow("Edges", edges)
cv2.imshow("Laser Mask", laser_mask)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
cv2.destroyAllWindows()
break
elif key == ord(' '):
servo_enabled = not servo_enabled
print(f"舵机控制 {'已启用' if servo_enabled else '已禁用'}")
elif key == ord('r'):
servo_controller.reset()
apply_servo_control(pwm_x_obj, pwm_y_obj, 1500, 1500)
print("舵机已重置到中心位置")
elif key == ord('c'):
servo_controller = ServoController(min_angle_x=-30, max_angle_x=50,
min_angle_y=-10, max_angle_y=10,
center_pwm_x=1500, center_pwm_y=1500)
apply_servo_control(pwm_x_obj, pwm_y_obj, 1500, 1500)
print("舵机已校准到中心位置")
elif key == ord('t'):
laser_in_zone = False
laser_duration = 0
print("激光计时已重置")
apply_servo_control(pwm_x_obj, pwm_y_obj, 0, 0)
cap.release()
cv2.destroyAllWindows()
cleanup_gpio(pwm_x_obj, pwm_y_obj)
if __name__ == "__main__":
main()
最新发布
本文提供了一套完整的嵌入式Linux学习方案，包括准备工作、基础课程、学习方法、实战环节和专业指导，旨在帮助初学者快速成为嵌入式Linux工程师。
扫一扫
7115
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
