import cv2
import numpy as np
import smbus
import time
import RPi.GPIO as GPIO
import signal
import sys
# ---------------------- 硬件配置 ----------------------
MOTOR_MODEL_ADDR = 0x26 # I2C从机地址
I2C_BUS = 1 # I2C总线号
TRIG = 23 # 超声波触发引脚
ECHO = 24 # 超声波接收引脚
MAX_SPEED = 1000 # 最大电机速度
SEARCH_SPEED = 100 # 搜索旋转速度
TRACKING_SPEED = 500 # 追踪速度
IMPACT_SPEED = 700 # 撞击速度
TARGET_DISTANCE = 120 # 黄色目标检测距离阈值
TARGET_MIN_AREA = 300 # 最小目标面积
IMPACT_DISTANCE = 25 # 撞击终止距离
STOP_DISTANCE = 15 # 紧急停止距离
OBSTACLE_DISTANCE = 30 # 障碍物检测距离(非黄色物体触发避障)
AVOID_BACK_SPEED = -400 # 避障后退速度
AVOID_TURN_SPEED = 150 # 避障转弯速度
AVOID_DURATION = 0.5 # 后退持续时间(秒)
TURN_DURATION = 0.8 # 转弯持续时间(秒)
IMPACT_BACK_DURATION = 0.3 # 撞击后后退持续时间
IMPACT_TURN_DURATION = 0.5 # 撞击后转弯持续时间
# PID参数
KP = 0.6
KI = 0.003
KD = 0.25
PID_OUTPUT_LIMIT = 300
SPEED_SMOOTH_FACTOR = 0.6
# 编码器配置
MOTOR_TYPE = 1
UPLOAD_DATA = 1
# I2C寄存器定义
MOTOR_TYPE_REG = 0x01
MOTOR_DEADZONE_REG = 0x02
MOTOR_PLUSELINE_REG = 0x03
MOTOR_PLUSEPHASE_REG = 0x04
WHEEL_DIA_REG = 0x05
SPEED_CONTROL_REG = 0x06
PWM_CONTROL_REG = 0x07
READ_TEN_M1_ENCODER_REG = 0x10
READ_TEN_M2_ENCODER_REG = 0x11
READ_TEN_M3_ENCODER_REG = 0x12
READ_TEN_M4_ENCODER_REG = 0x13
READ_ALLHIGH_M1_REG = 0x20
READ_ALLLOW_M1_REG = 0x21
READ_ALLHIGH_M2_REG = 0x22
READ_ALLLOW_M2_REG = 0x23
READ_ALLHIGH_M3_REG = 0x24
READ_ALLLOW_M3_REG = 0x25
READ_ALLHIGH_M4_REG = 0x26
READ_ALLLOW_M4_REG = 0x27
# 全局变量
encoder_offset = [0] * 4
encoder_now = [0] * 4
last_encoder = [0] * 4
wheel_circumference = 0.0
encoder_resolution = 0
last_motor_speeds = [0, 0, 0, 0] # 保存上次电机速度用于平滑控制
# ---------------------- I2C工具函数 ----------------------
def get_motor_bus():
"""获取电机控制的I2C总线实例"""
return smbus.SMBus(I2C_BUS)
def i2c_write(reg, data, retries=3):
"""安全写入I2C寄存器,带重试机制"""
for _ in range(retries):
try:
bus = get_motor_bus()
bus.write_i2c_block_data(MOTOR_MODEL_ADDR, reg, data)
return True
except Exception as e:
print(f"I2C写入错误,重试中: {e}")
return False
def i2c_read(reg, length, retries=3):
"""安全读取I2C寄存器,带重试机制"""
for _ in range(retries):
try:
bus = get_motor_bus()
return bus.read_i2c_block_data(MOTOR_MODEL_ADDR, reg, length)
except Exception as e:
print(f"I2C读取错误,重试中: {e}")
return []
# 浮点数转字节
def float_to_bytes(f):
import struct
return struct.pack('<f', f) # 小端字节序
# ---------------------- 电机配置函数 ----------------------
def set_motor_type(data):
i2c_write(MOTOR_TYPE_REG, [data])
def set_motor_deadzone(data):
buf = [(data >> 8) & 0xFF, data & 0xFF]
i2c_write(MOTOR_DEADZONE_REG, buf)
def set_pluse_line(data):
buf = [(data >> 8) & 0xFF, data & 0xFF]
i2c_write(MOTOR_PLUSELINE_REG, buf)
def set_pluse_phase(data):
buf = [(data >> 8) & 0xFF, data & 0xFF]
i2c_write(MOTOR_PLUSEPHASE_REG, buf)
def set_wheel_dis(data):
bytes_data = float_to_bytes(data)
i2c_write(WHEEL_DIA_REG, list(bytes_data))
def set_motor_parameter():
"""配置电机参数,仅需设置一次"""
global wheel_circumference, encoder_resolution
if MOTOR_TYPE == 1:
set_motor_type(1)
time.sleep(0.1)
set_pluse_phase(30)
time.sleep(0.1)
set_pluse_line(11)
time.sleep(0.1)
set_wheel_dis(67.00)
time.sleep(0.1)
set_motor_deadzone(1600)
time.sleep(0.1)
wheel_circumference = 6.7 * np.pi
encoder_resolution = 11 * 30
elif MOTOR_TYPE == 2:
set_motor_type(2)
time.sleep(0.1)
set_pluse_phase(20)
time.sleep(0.1)
set_pluse_line(13)
time.sleep(0.1)
set_wheel_dis(48.00)
time.sleep(0.1)
set_motor_deadzone(1200)
time.sleep(0.1)
wheel_circumference = 4.8 * np.pi
encoder_resolution = 13 * 20
# 其他电机类型配置...
# ---------------------- 电机控制函数 ----------------------
def set_motor_speed(speed1, speed2, speed3, speed4):
"""设置四个电机的速度,增加平滑控制"""
global last_motor_speeds
# 平滑速度变化,减少突变
speed1 = int(last_motor_speeds[0] * (1-SPEED_SMOOTH_FACTOR) + speed1 * SPEED_SMOOTH_FACTOR)
speed2 = int(last_motor_speeds[1] * (1-SPEED_SMOOTH_FACTOR) + speed2 * SPEED_SMOOTH_FACTOR)
speed3 = int(last_motor_speeds[2] * (1-SPEED_SMOOTH_FACTOR) + speed3 * SPEED_SMOOTH_FACTOR)
speed4 = int(last_motor_speeds[3] * (1-SPEED_SMOOTH_FACTOR) + speed4 * SPEED_SMOOTH_FACTOR)
# 限制速度范围
speed1 = int(max(-MAX_SPEED, min(MAX_SPEED, speed1)))
speed2 = int(max(-MAX_SPEED, min(MAX_SPEED, speed2)))
speed3 = int(max(-MAX_SPEED, min(MAX_SPEED, speed3)))
speed4 = int(max(-MAX_SPEED, min(MAX_SPEED, speed4)))
speeds = [
(speed1 >> 8) & 0xFF, speed1 & 0xFF,
(speed2 >> 8) & 0xFF, speed2 & 0xFF,
(speed3 >> 8) & 0xFF, speed3 & 0xFF,
(speed4 >> 8) & 0xFF, speed4 & 0xFF
]
# 保存当前速度用于下次平滑
last_motor_speeds = [speed1, speed2, speed3, speed4]
return i2c_write(SPEED_CONTROL_REG, speeds)
def stop_motors():
"""安全停止所有电机"""
# 逐步减速停止,避免急停
for i in range(5):
factor = 1.0 - (i * 0.2)
set_motor_speed(
int(last_motor_speeds[0] * factor),
int(last_motor_speeds[1] * factor),
int(last_motor_speeds[2] * factor),
int(last_motor_speeds[3] * factor)
)
time.sleep(0.05)
# 最终确认停止
set_motor_speed(0, 0, 0, 0)
time.sleep(0.05)
set_motor_speed(0, 0, 0, 0)
# ---------------------- 编码器读取函数 ----------------------
def read_10_encoder():
"""读取实时编码器数据(相对值)"""
global encoder_offset
for i in range(4):
reg = READ_TEN_M1_ENCODER_REG + i
buf = i2c_read(reg, 2)
if len(buf) != 2:
continue
encoder_offset[i] = (buf[0] << 8) | buf[1]
if encoder_offset[i] & 0x8000:
encoder_offset[i] -= 0x10000
return encoder_offset
def read_all_encoder():
"""读取累计编码器数据(绝对值)"""
global encoder_now
for i in range(4):
high_reg = READ_ALLHIGH_M1_REG + (i * 2)
low_reg = READ_ALLLOW_M1_REG + (i * 2)
high_buf = i2c_read(high_reg, 2)
low_buf = i2c_read(low_reg, 2)
if len(high_buf) != 2 or len(low_buf) != 2:
continue
high_val = (high_buf[0] << 8) | high_buf[1]
low_val = (low_buf[0] << 8) | low_buf[1]
encoder_val = (high_val << 16) | low_val
if encoder_val >= 0x80000000:
encoder_val -= 0x100000000
encoder_now[i] = encoder_val
return encoder_now
def get_wheel_distances():
"""获取四个轮子的行驶距离(单位:cm)"""
encoders = read_all_encoder() if UPLOAD_DATA == 1 else read_10_encoder()
distances = [e * wheel_circumference / encoder_resolution for e in encoders]
return distances
# ---------------------- 传感器函数 ----------------------
def setup_sensors():
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(TRIG, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
def get_distance():
"""获取超声波距离(单位:厘米),带滤波"""
distances = []
for _ in range(5): # 采样5次
try:
# 发送触发信号
GPIO.output(TRIG, False)
time.sleep(0.005)
GPIO.output(TRIG, True)
time.sleep(0.00001)
GPIO.output(TRIG, False)
# 等待回声信号
pulse_start = time.time()
while GPIO.input(ECHO) == 0 and time.time() - pulse_start < 0.1:
pulse_start = time.time()
if time.time() - pulse_start >= 0.1:
continue # 超时,跳过此次测量
pulse_end = time.time()
while GPIO.input(ECHO) == 1 and time.time() - pulse_end < 0.1:
pulse_end = time.time()
if time.time() - pulse_end >= 0.1:
continue # 超时,跳过此次测量
# 计算距离
pulse_duration = pulse_end - pulse_start
distance = pulse_duration * 17150 # 声速34300cm/s,单程
if 2 <= distance <= 400: # 有效距离范围
distances.append(distance)
except Exception as e:
print(f"超声波读取错误: {e}")
if distances:
return round(np.median(distances), 2) # 返回中值
return None
# ---------------------- 视觉检测 ----------------------
def detect_yellow_targets(frame):
"""检测所有黄色目标并返回中心坐标,按距离排序"""
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# 创建黄色掩码(扩大范围以提高检测率)
lower_yellow = np.array([20, 100, 100]) # 黄色范围下界
upper_yellow = np.array([40, 255, 255]) # 黄色范围上界
# 应用掩码
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
# 形态学操作,减少噪声
kernel = np.ones((5, 5), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
# 寻找轮廓
contours, _ = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
targets = []
if not contours:
return targets, frame, mask
# 遍历所有轮廓,找到所有符合条件的目标
for contour in contours:
area = cv2.contourArea(contour)
if area < TARGET_MIN_AREA: # 过滤小目标
continue
# 计算轮廓的边界圆和中心
((x, y), radius) = cv2.minEnclosingCircle(contour)
M = cv2.moments(contour)
if M["m00"] != 0:
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
else:
cX, cY = int(x), int(y)
# 估算目标距离(基于半径大小)
estimated_distance = 500 / radius # 简单估算,实际应用中可能需要更复杂的模型
# 保存目标信息
targets.append({
'center': (cX, cY),
'radius': int(radius),
'area': area,
'estimated_distance': estimated_distance
})
# 在原图上绘制检测结果
cv2.circle(frame, (cX, cY), int(radius), (0, 255, 255), 2) # 黄色轮廓
cv2.putText(frame, f"Target {len(targets)}", (cX - 30, cY - 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 2)
cv2.putText(frame, f"Area: {int(area)}", (cX - 30, cY - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 2)
# 按估算距离排序(从小到大)
targets.sort(key=lambda t: t['estimated_distance'])
return targets, frame, mask
def detect_non_yellow_obstacles(frame):
"""检测非黄色障碍物并返回障碍物信息"""
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# 创建黄色掩码
lower_yellow = np.array([20, 100, 100])
upper_yellow = np.array([40, 255, 255])
yellow_mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
# 创建非黄色掩码(取反)
non_yellow_mask = cv2.bitwise_not(yellow_mask)
# 形态学操作,减少噪声
kernel = np.ones((5, 5), np.uint8)
non_yellow_mask = cv2.morphologyEx(non_yellow_mask, cv2.MORPH_OPEN, kernel)
non_yellow_mask = cv2.morphologyEx(non_yellow_mask, cv2.MORPH_CLOSE, kernel)
# 寻找轮廓
contours, _ = cv2.findContours(non_yellow_mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
obstacles = []
if not contours:
return obstacles, frame, non_yellow_mask
# 遍历所有轮廓,找到所有符合条件的障碍物
for contour in contours:
area = cv2.contourArea(contour)
if area < TARGET_MIN_AREA: # 过滤小目标
continue
# 计算轮廓的边界矩形和中心
x, y, w, h = cv2.boundingRect(contour)
cX = x + w // 2
cY = y + h // 2
# 保存障碍物信息
obstacles.append({
'center': (cX, cY),
'width': w,
'height': h,
'area': area,
'position': 'left' if cX < 320 else 'right' # 障碍物在图像左侧还是右侧
})
# 在原图上绘制检测结果
cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 0, 255), 2) # 红色矩形
cv2.putText(frame, f"Obstacle {len(obstacles)}", (x, y - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
return obstacles, frame, non_yellow_mask
def analyze_post_impact(frame):
"""分析撞击后画面,决定左转还是右转"""
targets, _, _ = detect_yellow_targets(frame)
if not targets:
return "left" # 默认左转
# 获取最大的目标
main_target = max(targets, key=lambda t: t['area'])
target_x = main_target['center'][0]
# 如果目标在左侧,左转;在右侧,右转
if target_x < 320: # 图像中心x坐标
return "left"
else:
return "right"
# ---------------------- PID控制器类 ----------------------
class PIDController:
def __init__(self, kp, ki, kd, output_limit):
self.kp = kp
self.ki = ki
self.kd = kd
self.output_limit = output_limit
self.reset()
def reset(self):
"""重置控制器状态"""
self.last_error = 0.0
self.integral = 0.0
self.derivative = 0.0
self.last_output = 0.0 # 保存上次输出值
def update(self, error, dt=0.02):
"""计算PID输出,增加输出平滑"""
# 计算积分项
self.integral += error * dt
self.integral = max(-self.output_limit, min(self.output_limit, self.integral))
# 计算微分项(使用低通滤波减少噪声)
derivative = (error - self.last_error) / dt if dt > 0 else 0
self.derivative = 0.7 * self.derivative + 0.3 * derivative # 低通滤波
# 计算PID输出
p_term = self.kp * error
i_term = self.ki * self.integral
d_term = self.kd * self.derivative
# 输出平滑(防止突变)
output = p_term + i_term + d_term
smooth_output = 0.8 * self.last_output + 0.2 * output # 平滑因子
smooth_output = max(-self.output_limit, min(self.output_limit, smooth_output))
# 更新状态
self.last_error = error
self.last_output = smooth_output
return smooth_output
# ---------------------- 控制逻辑 ----------------------
class YellowBoxAttacker:
def __init__(self):
self.state = "SEARCHING" # 当前状态:SEARCHING/ TRACKING/ IMPACTING/ AVOIDING/ POST_IMPACT
self.search_direction = 1 # 搜索方向
self.avoid_direction = 1 # 避障转弯方向(1=右转,-1=左转)
self.post_impact_direction = 1 # 撞击后转弯方向
self.search_start_time = time.time()
self.last_detected_time = 0
self.last_encoder_pos = [0, 0, 0, 0]
self.distance_traveled = [0, 0, 0, 0]
self.pid_controller = PIDController(KP, KI, KD, PID_OUTPUT_LIMIT)
self.last_pid_time = time.time()
self.target_detected_frames = 0
self.target_lost_frames = 0
self.min_detection_frames = 2
self.min_lost_frames = 5
self.impact_start_time = 0
self.impact_distance = 0
self.avoid_start_time = 0
self.post_impact_start_time = 0 # 撞击后动作开始时间
def update_encoder_position(self):
"""更新编码器位置和行驶距离"""
current_encoders = read_all_encoder()
for i in range(4):
self.distance_traveled[i] = (current_encoders[i] - self.last_encoder_pos[i]) * \
wheel_circumference / encoder_resolution
self.last_encoder_pos = current_encoders.copy()
def reset_encoder_position(self):
"""重置编码器位置计数"""
self.last_encoder_pos = read_all_encoder().copy()
self.distance_traveled = [0, 0, 0, 0]
def search_for_target(self):
"""优化的搜索算法,减少抖动"""
# 每6秒切换一次搜索方向
if time.time() - self.search_start_time > 6:
self.search_direction *= -1
self.search_start_time = time.time()
self.reset_encoder_position()
print(f"切换搜索方向: {'顺时针' if self.search_direction > 0 else '逆时针'}")
# 使用平滑加速启动
elapsed_time = time.time() - self.search_start_time
if elapsed_time < 0.5: # 加速阶段
speed_factor = min(1.0, elapsed_time * 2) # 0-0.5秒内从0加速到100%
else: # 稳定阶段
speed_factor = 1.0
# 左右轮差速控制旋转
set_motor_speed(
int(SEARCH_SPEED * speed_factor * self.search_direction),
int(SEARCH_SPEED * speed_factor * self.search_direction),
int(-SEARCH_SPEED * speed_factor * self.search_direction),
int(-SEARCH_SPEED * speed_factor * self.search_direction)
)
self.update_encoder_position()
print(f"搜索目标中...速度: {SEARCH_SPEED * speed_factor:.1f}, 编码器: {self.last_encoder_pos}")
def track_with_pid(self, target, distance):
"""使用PID控制进行追踪,增加稳定性优化"""
current_time = time.time()
dt = current_time - self.last_pid_time
if dt < 0.01: dt = 0.01 # 防止除零
# 计算目标在图像中的水平偏移(误差)
error = target['center'][0] - 320 # 图像中心x坐标
# 距离调整系数(远距离时降低灵敏度,近距离时提高灵敏度)
distance_factor = min(1.5, max(0.7, 1.0 - (distance - 30) * 0.02))
# 更新PID控制器(应用距离调整)
pid_output = self.pid_controller.update(error * distance_factor, dt)
# 根据距离和目标大小调整基础速度
if distance > 60: # 远距离追踪
base_speed = TRACKING_SPEED * 0.8
print(f"远距离追踪 - 距离: {distance}cm, 速度系数: {distance_factor:.2f}")
elif distance > IMPACT_DISTANCE: # 中距离追踪
base_speed = TRACKING_SPEED
print(f"中距离追踪 - 距离: {distance}cm, 速度系数: {distance_factor:.2f}")
else: # 近距离撞击
base_speed = IMPACT_SPEED * min(1.0, distance / IMPACT_DISTANCE) # 接近时减速
print(f"准备撞击 - 距离: {distance}cm, 速度系数: {distance_factor:.2f}")
# 记录撞击开始时间和距离
if self.state != "IMPACTING":
self.state = "IMPACTING"
self.impact_start_time = time.time()
self.impact_distance = distance
# 差速控制计算
left_speed = base_speed - pid_output
right_speed = base_speed + pid_output
# 速度限幅
left_speed = max(-MAX_SPEED, min(MAX_SPEED, left_speed))
right_speed = max(-MAX_SPEED, min(MAX_SPEED, right_speed))
# 设置电机速度
set_motor_speed(
int(left_speed), # 左前
int(left_speed), # 左后
int(right_speed), # 右前
int(right_speed) # 右后
)
# 打印调试信息
print(f"误差: {error}, PID输出: {pid_output:.2f}, 左速: {left_speed}, 右速: {right_speed}")
print(f"积分项: {self.pid_controller.integral:.2f}, 微分项: {self.pid_controller.derivative:.2f}")
self.last_pid_time = current_time
def select_target(self, targets, ultrasonic_distance):
"""选择最佳目标(距离最近的)"""
if not targets:
return None
# 如果超声波距离有效,优先使用
if ultrasonic_distance is not None:
# 计算每个目标的综合距离评分
for target in targets:
# 结合视觉估算距离和超声波距离
visual_distance = target['estimated_distance']
# 加权平均,超声波距离权重更高
target['combined_distance'] = 0.3 * visual_distance + 0.7 * ultrasonic_distance
# 按综合距离排序
targets.sort(key=lambda t: t['combined_distance'])
return targets[0]
# 如果没有超声波数据,使用视觉估算距离
return targets[0]
def avoid_obstacle(self):
"""执行后退+转弯避障动作"""
current_time = time.time()
elapsed = current_time - self.avoid_start_time
if elapsed < AVOID_DURATION:
# 阶段1:后退
set_motor_speed(
AVOID_BACK_SPEED,
AVOID_BACK_SPEED,
AVOID_BACK_SPEED,
AVOID_BACK_SPEED
)
print("检测到障碍物,正在后退...")
elif elapsed < AVOID_DURATION + TURN_DURATION:
# 阶段2:转弯(随机选择方向)
if elapsed == AVOID_DURATION: # 只在切换阶段时选择方向
self.avoid_direction = 1 if np.random.rand() > 0.5 else -1 # 随机左右转
set_motor_speed(
AVOID_TURN_SPEED * self.avoid_direction,
AVOID_TURN_SPEED * self.avoid_direction,
-AVOID_TURN_SPEED * self.avoid_direction,
-AVOID_TURN_SPEED * self.avoid_direction
)
print(f"后退完成,正在{'右' if self.avoid_direction>0 else '左'}转避开障碍物")
else:
# 避障完成,返回搜索状态
self.state = "SEARCHING"
self.search_start_time = time.time()
print("避障完成,继续搜索目标")
def process_post_impact(self, frame):
"""处理撞击后的动作(后退+转向)"""
current_time = time.time()
elapsed = current_time - self.post_impact_start_time
if elapsed < IMPACT_BACK_DURATION:
# 阶段1:撞击后小幅度后退
set_motor_speed(
-200, # 后退速度
-200,
-200,
-200
)
print("撞击完成,正在后退...")
elif elapsed < IMPACT_BACK_DURATION + IMPACT_TURN_DURATION:
# 阶段2:根据分析结果转向
if elapsed == IMPACT_BACK_DURATION: # 只在切换阶段时分析画面
self.post_impact_direction = analyze_post_impact(frame)
print(f"分析画面后决定{'左转' if self.post_impact_direction == 'left' else '右转'}")
turn_speed = AVOID_TURN_SPEED
if self.post_impact_direction == "left":
set_motor_speed(
-turn_speed, # 左转
-turn_speed,
turn_speed,
turn_speed
)
else:
set_motor_speed(
turn_speed, # 右转
turn_speed,
-turn_speed,
-turn_speed
)
else:
# 完成撞击后动作,返回搜索状态
self.state = "SEARCHING"
self.search_start_time = time.time()
print("撞击后动作完成,继续搜索目标")
def run(self):
setup_sensors()
set_motor_parameter()
cap = cv2.VideoCapture(0)
cap.set(3, 640)
cap.set(4, 480)
print("启动黄色盒子撞击程序,支持障碍物规避")
try:
while True:
ret, frame = cap.read()
if not ret:
break
# 检测黄色目标
targets, frame, yellow_mask = detect_yellow_targets(frame)
# 检测非黄色障碍物
obstacles, frame, obstacle_mask = detect_non_yellow_obstacles(frame)
ultrasonic_distance = get_distance()
best_target = self.select_target(targets, ultrasonic_distance)
# 显示状态和距离
cv2.putText(frame, f"State: {self.state}", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
# 障碍物检测逻辑
if self.state == "SEARCHING" or self.state == "TRACKING":
# 检查超声波距离和非黄色障碍物
obstacle_detected = False
if ultrasonic_distance is not None and ultrasonic_distance < OBSTACLE_DISTANCE:
obstacle_detected = True
elif obstacles:
# 如果检测到非黄色障碍物且面积足够大
largest_obstacle = max(obstacles, key=lambda o: o['area'])
if largest_obstacle['area'] > TARGET_MIN_AREA * 2:
obstacle_detected = True
if obstacle_detected:
print(f"检测到障碍物!距离:{ultrasonic_distance}cm")
self.state = "AVOIDING"
self.avoid_start_time = time.time()
self.pid_controller.reset() # 重置PID状态
# 状态机处理
if self.state == "AVOIDING":
self.avoid_obstacle() # 执行避障动作
elif self.state == "POST_IMPACT":
self.process_post_impact(frame) # 处理撞击后动作
elif best_target and ultrasonic_distance is not None and ultrasonic_distance <= TARGET_DISTANCE:
# 黄色目标检测逻辑
self.last_detected_time = time.time()
self.target_detected_frames += 1
self.target_lost_frames = 0
if self.target_detected_frames >= self.min_detection_frames:
if self.state == "SEARCHING":
print(f"切换到追踪状态 - 目标距离:{ultrasonic_distance}cm")
self.state = "TRACKING"
self.track_with_pid(best_target, ultrasonic_distance)
if self.state == "IMPACTING":
if ultrasonic_distance < STOP_DISTANCE or time.time() - self.impact_start_time > 2:
stop_motors()
print("撞击完成,分析画面决定下一步动作")
self.state = "POST_IMPACT"
self.post_impact_start_time = time.time()
else:
# 丢失目标或搜索状态处理
self.target_detected_frames = 0
self.target_lost_frames += 1
if self.target_lost_frames >= self.min_lost_frames and self.state != "AVOIDING":
print("丢失目标,恢复搜索")
self.state = "SEARCHING"
self.search_for_target()
# 显示画面
cv2.putText(frame, f"Targets: {len(targets)}", (10, 120),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
if ultrasonic_distance is not None:
cv2.putText(frame, f"Ultrasonic: {ultrasonic_distance:.1f}cm", (10, 90),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
cv2.imshow('Yellow Target Detection', frame)
cv2.imshow('Yellow Mask', yellow_mask)
cv2.imshow('Obstacle Mask', obstacle_mask)
# 按'q'键退出
if cv2.waitKey(1) & 0xFF == ord('q'):
break
except KeyboardInterrupt:
print("\n程序中断")
finally:
stop_motors()
cap.release()
cv2.destroyAllWindows()
GPIO.cleanup()
# 信号处理函数
def signal_handler(sig, frame):
print(f"\n接收到信号 {sig},准备安全退出...")
stop_motors()
GPIO.cleanup([TRIG, ECHO])
sys.exit(0)
if __name__ == "__main__":
# 注册信号处理函数
signal.signal(signal.SIGINT, signal_handler)
signal.signal(signal.SIGTERM, signal_handler)
# 创建并运行黄色盒子攻击者
attacker = YellowBoxAttacker()
attacker.run()第一,此避障功能过强,请修改非黄色物块距离小于二十厘米时候实现避障,其余时候探索目标物体,第二,将颜色阈值改高一些,比如只能亮黄色明黄色能探测,第三,最后记得把停止功能调整一下,因为运行前代码时候使用终端命令停止小车时,电机依旧在运行,请强化停止运行的功能,最后,请满足三个条件,调整修改代码,确保代码的完整性和可实施性
最新发布