一前言
大家好，好久不见！断更太久了，因为最近一直在准备蓝桥杯的事情，而大家又不喜欢看所以我就没在更新了，等月底我会更新一个关于我自己做算法题的时候总结的题型和模板，希望能帮助到大家。这次更新的是我一直在做的项目，简单的和大家说一说。
二主要内容
硬件说明
我用的还是最简单的设备，因为我还没买香橙派，树莓派，总线舵机（其实买了，不会调，转换器还要100多没钱啊。。）所以只能尽我所能用代码的精度弥补设备了
主板是嘉立创的k230，云台的舵机是mg966r，精度比较低，垂直舵机是180度的，水平舵机是360的（这样可以实现开机自启动，恢复默认位置）剩下的就都是嘉立创的屏幕，摄像头。
代码展示(剩下的代码可以去我的github查看，会持续更新我的GitHub）
这里只放两段我最新的也是效果最好的代码(python)
java
import time
from media.sensor import *
from media.display import *
from media.media import *
from machine import PWM, FPIOA
# ========== 基本参数 ==========
W, H = 800, 480
CX, CY = W // 2, H // 2
# 舵机参数
SERVO_MIN_NS = 1000000
SERVO_MID_NS = 1500000
SERVO_MAX_NS = 2000000
# 垂直舵机角度范围(*100)
TILT_MIN, TILT_MAX, TILT_INIT = 4500, 13500, 9000
# 红色阈值
RED_TH = (20, 80, 30, 100, 0, 60)
# ===== 稳定性控制参数 =====
DEADZONE = 10 # 死区(像素) - 在此范围内不动
SMOOTH = 0.4 # 平滑系数(0~1, 越小越稳)
PAN_SCALE = 9.0 # 水平灵敏度
TILT_SCALE = 12.5 # 垂直灵敏度
MIN_MOVE = 75 # 最小移动量
# 距离参数
DIST_REF = 5000 # 参考像素数
# ========== 全局状态 ==========
tilt_angle = TILT_INIT
smooth_x, smooth_y = 0.0, 0.0
pixel_avg = DIST_REF
def angle_to_ns(angle):
angle = max(TILT_MIN, min(TILT_MAX, angle))
return SERVO_MIN_NS + (angle * 1000000) // 18000
def speed_to_ns(speed):
speed = max(-10000, min(10000, speed))
return SERVO_MID_NS + speed * 50
def init_hw():
# 舵机
FPIOA().set_function(46, FPIOA.PWM2)
FPIOA().set_function(47, FPIOA.PWM3)
ud = PWM(2, freq=50)
lr = PWM(3, freq=50)
ud.duty_ns(angle_to_ns(TILT_INIT))
lr.duty_ns(SERVO_MID_NS)
# 摄像头
s = Sensor(width=W, height=H)
s.reset()
s.set_framesize(width=W, height=H)
s.set_pixformat(Sensor.RGB565)
Display.init(Display.ST7701, width=W, height=H, to_ide=True)
MediaManager.init()
s.run()
time.sleep(0.5)
return lr, ud, s
def main():
global tilt_angle, smooth_x, smooth_y, pixel_avg
print("实时追踪模式启动")
lr, ud, cam = init_hw()
try:
while True:
img = cam.snapshot()
blobs = img.find_blobs([RED_TH], pixels_threshold=200, merge=True)
if blobs:
b = max(blobs, key=lambda x: x.pixels())
x, y, px = b.cx(), b.cy(), b.pixels()
# 原始误差
raw_x = x - CX
raw_y = y - CY
# 低通滤波平滑
smooth_x = smooth_x * (1 - SMOOTH) + raw_x * SMOOTH
smooth_y = smooth_y * (1 - SMOOTH) + raw_y * SMOOTH
# 像素数平滑(距离)
pixel_avg = pixel_avg * 0.9 + px * 0.1
# === 水平控制 ===
if abs(smooth_x) > DEADZONE:
spd = -smooth_x * PAN_SCALE
if spd > 0:
spd = max(spd, MIN_MOVE)
else:
spd = min(spd, -MIN_MOVE)
lr.duty_ns(speed_to_ns(int(spd)))
else:
lr.duty_ns(SERVO_MID_NS)
# === 垂直控制 ===
if abs(smooth_y) > DEADZONE:
tilt_angle -= smooth_y * TILT_SCALE * SMOOTH
tilt_angle = max(TILT_MIN, min(TILT_MAX, tilt_angle))
ud.duty_ns(angle_to_ns(int(tilt_angle)))
# 绘制
img.draw_rectangle(b.rect(), color=(255,0,0), thickness=2)
img.draw_cross(x, y, color=(255,0,0), size=10)
img.draw_line(x, y, CX, CY, color=(255,200,0))
# 距离显示
dist_ratio = int(pixel_avg / DIST_REF * 100)
if dist_ratio > 120:
dt, dc = "近", (0,255,0)
elif dist_ratio < 80:
dt, dc = "远", (255,100,100)
else:
dt, dc = "中", (255,255,0)
img.draw_string_advanced(20, 80, 28, f"距离:{dt} {dist_ratio}%", color=dc)
img.draw_string_advanced(20, 40, 32, "锁定中", color=(0,255,255))
else:
lr.duty_ns(SERVO_MID_NS)
smooth_x, smooth_y = 0, 0
img.draw_string_advanced(20, 40, 32, "搜索中", color=(255,150,150))
# 中心准星
img.draw_cross(CX, CY, color=(0,255,0), size=25, thickness=3)
img.draw_circle(CX, CY, 40, color=(0,255,0), thickness=2)
Display.show_image(img)
time.sleep_ms(30)
except KeyboardInterrupt:
pass
finally:
lr.duty_ns(SERVO_MID_NS)
ud.duty_ns(angle_to_ns(TILT_INIT))
print("停止")
if __name__ == "__main__":
main()
java
import time
from media.sensor import *
from media.display import *
from media.media import *
from machine import PWM, FPIOA
# ========== 配置参数 ==========
W, H = 800, 480 # 屏幕尺寸
CX, CY = W // 2, H // 2 # 中心点
# 安全区设置(高度=屏幕高度, 宽度=屏幕宽度的3/5)
SAFE_AREA_W = int(W * 3 / 5) # 安全区宽度(屏幕宽度的3/5)
SAFE_AREA_H = H # 安全区高度(屏幕高度)
SAFE_AREA_X_MIN = (W - SAFE_AREA_W) // 2
SAFE_AREA_X_MAX = SAFE_AREA_X_MIN + SAFE_AREA_W
SAFE_AREA_Y_MIN = (H - SAFE_AREA_H) // 2
SAFE_AREA_Y_MAX = SAFE_AREA_Y_MIN + SAFE_AREA_H
# 舵机脉宽(纳秒)
SERVO_MIN_NS = 1000000 # 1.0ms
SERVO_MID_NS = 1500000 # 1.5ms
SERVO_MAX_NS = 2000000 # 2.0ms
# 垂直舵机(180度) - 角度控制
TILT_ANGLE_MIN = 4500 # 45度*100
TILT_ANGLE_MAX = 13500 # 135度*100
TILT_ANGLE_INIT = 9000 # 90度*100
# 水平舵机(360度) - 速度控制
PAN_SPEED_MIN = -10000 # 最大左转*100
PAN_SPEED_MAX = 10000 # 最大右转*100
# 跟踪参数
RED_THRESHOLD = (20, 80, 30, 100, 0, 60)
MIN_PIXELS = 200
LOST_TARGET_DELAY = 90 # 3秒延迟(30fps)
# 控制系数
PAN_COEF = 0.5 # 水平比例系数 (在安全区内使用)
PAN_COEF_OUTSIDE = 1.5 # 水平比例系数 (离开安全区时使用,更快响应)
TILT_COEF = 15 # 垂直比例系数
PAN_DEADZONE = 800 # 水平死区 (增大以减少抖动)
PAN_MIN_SPEED = 500 # 最小转动速度
PAN_MIN_SPEED_OUTSIDE = 1500 # 最小转动速度 (离开安全区时使用,更快响应)
# 前后距离识别参数
DISTANCE_REFERENCE_PIXELS = 5000 # 参考像素数(标准距离)
DISTANCE_DEADZONE = 500 # 距离死区(像素数)
# ========== 转换函数 ==========
def tilt_angle_to_ns(angle):
"""垂直角度转脉宽"""
angle = max(TILT_ANGLE_MIN, min(TILT_ANGLE_MAX, angle))
ns = SERVO_MIN_NS + ((angle * 1000000) // 18000)
return max(SERVO_MIN_NS, min(SERVO_MAX_NS, ns))
def pan_speed_to_ns(speed):
"""水平速度转脉宽"""
speed = max(PAN_SPEED_MIN, min(PAN_SPEED_MAX, speed))
ns = SERVO_MID_NS + (speed * 50)
return max(SERVO_MIN_NS, min(SERVO_MAX_NS, ns))
def analyze_distance(pixels):
"""
分析物体前后距离
:param pixels: 物体像素数
:return: distance_state ('FORWARD', 'MIDDLE', 'BACKWARD')
:return: distance_value: 距离差值(像素数)
"""
diff = pixels - DISTANCE_REFERENCE_PIXELS
if abs(diff) < DISTANCE_DEADZONE:
return 'MIDDLE', diff # 接近标准距离
elif diff > 0:
return 'FORWARD', diff # 物体越来越近
else:
return 'BACKWARD', diff # 物体越来越远
# ========== 初始化 ==========
def init_servos():
"""初始化舵机"""
print("初始化舵机...")
# 垂直舵机(GPIO46 -> PWM2)
pwm_io1 = FPIOA()
pwm_io1.set_function(46, FPIOA.PWM2)
pwm_ud = PWM(2, freq=50)
pwm_ud.duty_ns(tilt_angle_to_ns(TILT_ANGLE_INIT))
# 水平舵机(GPIO47 -> PWM3)
pwm_io2 = FPIOA()
pwm_io2.set_function(47, FPIOA.PWM3)
pwm_lr = PWM(3, freq=50)
pwm_lr.duty_ns(SERVO_MID_NS)
print(f"垂直舵机(GPIO46): 90度")
print(f"水平舵机(GPIO47): 停止")
time.sleep(1)
return pwm_lr, pwm_ud
def init_camera():
"""初始化摄像头"""
print("初始化摄像头...")
sensor = Sensor(width=W, height=H)
sensor.reset()
sensor.set_framesize(width=W, height=H)
sensor.set_pixformat(Sensor.RGB565)
Display.init(Display.ST7701, width=W, height=H, to_ide=True)
MediaManager.init()
sensor.run()
return sensor
# ========== 主程序 ==========
def main():
print("=" * 40)
print("K230 云台红色物体跟踪")
print("含前后距离识别功能")
print("=" * 40)
# 初始化硬件
pwm_lr, pwm_ud = init_servos()
sensor = init_camera()
# 状态变量
frame_count = 0
last_frame_time = time.ticks_ms()
fps = 0
target_history = []
lost_target_frames = 0
print("\n开始跟踪...\n按Ctrl+C停止\n")
try:
error_count = 0 # 错误计数器
max_errors = 5 # 最大连续错误次数
while True:
try:
frame_start = time.ticks_ms()
# 捕获图像
img = sensor.snapshot()
# 寻找红色物块
blobs = img.find_blobs([RED_THRESHOLD],
pixels_threshold=MIN_PIXELS,
area_threshold=MIN_PIXELS,
merge=True)
if blobs:
# 找到目标
blob = max(blobs, key=lambda b: b.pixels())
x, y = blob.cx(), blob.cy()
pixels = blob.pixels() # 物体像素数
# 计算原始误差(像素)
delta_x = x - CX
delta_y = y - CY
# 分析前后距离
distance_state, distance_value = analyze_distance(pixels)
# 检查物体是否在安全区内
in_safe_area = (SAFE_AREA_X_MIN <= x <= SAFE_AREA_X_MAX and
SAFE_AREA_Y_MIN <= y <= SAFE_AREA_Y_MAX)
# 水平控制:只有在安全区外才追踪
if in_safe_area:
# 在安全区内,水平舵机停止转动
pan_speed = 0
else:
# 在安全区外,执行追踪(使用更高的速度)
# 物体在中央十字左侧(delta_x < 0) -> 云台向左(-速度)
# 物体在中央十字右侧(delta_x > 0) -> 云台向右(+速度)
# 死区检查:如果偏差较小,停止转动
if abs(delta_x) < PAN_DEADZONE // 100:
pan_speed = 0
else:
# 计算速度(离开安全区时使用更高系数,反向计算)
pan_speed_simple = (-delta_x * PAN_COEF_OUTSIDE)
pan_speed_simple = max(PAN_SPEED_MIN, min(PAN_SPEED_MAX, pan_speed_simple))
if pan_speed_simple > 0:
# 右侧偏差:云台向右转
pan_speed = max(pan_speed_simple, PAN_MIN_SPEED_OUTSIDE)
else:
# 左侧偏差:云台向左转
pan_speed = min(pan_speed_simple, -PAN_MIN_SPEED_OUTSIDE)
# 垂直控制:比例控制
tilt_offset = (-delta_y * TILT_COEF)
tilt_offset = max(-4500, min(4500, tilt_offset))
target_tilt_angle = TILT_ANGLE_INIT + tilt_offset
target_tilt_angle = max(TILT_ANGLE_MIN, min(TILT_ANGLE_MAX, target_tilt_angle))
# 控制舵机
pwm_lr.duty_ns(pan_speed_to_ns(pan_speed))
pwm_ud.duty_ns(tilt_angle_to_ns(target_tilt_angle))
# 绘制标记
img.draw_rectangle(blob.rect(), color=(255,0,0), thickness=2)
img.draw_cross(x, y, color=(255,0,0), size=10, thickness=2)
img.draw_line(x, y, CX, CY, color=(255,150,0), thickness=1)
# 绘制安全区
safe_area_color = (0,255,0) if in_safe_area else (255,100,0)
img.draw_rectangle((SAFE_AREA_X_MIN, SAFE_AREA_Y_MIN, SAFE_AREA_W, SAFE_AREA_H),
color=safe_area_color, thickness=2)
# 记录轨迹
target_history.append((x, y))
if len(target_history) > 20:
target_history.pop(0)
if len(target_history) > 1:
for i in range(1, len(target_history)):
x1, y1 = target_history[i-1]
x2, y2 = target_history[i]
img.draw_line(x1, y1, x2, y2, color=(255,100,0), thickness=1)
lost_target_frames = 0
# 显示前后距离信息
if distance_state == 'FORWARD':
distance_display = "物体靠近"
distance_color = (0,255,0) # 绿色
elif distance_state == 'BACKWARD':
distance_display = "物体远离"
distance_color = (255,0,0) # 红色
else:
distance_display = "距离适中"
distance_color = (0,255,255) # 黄色
img.draw_string_advanced(20, 100, 24, distance_display, color=distance_color)
img.draw_string_advanced(20, 130, 20, f"像素: {pixels}", color=(200,200,200))
else:
# 未找到目标
lost_target_frames += 1
# 立即停止水平舵机
pwm_lr.duty_ns(SERVO_MID_NS)
# 3秒后垂直舵机返回初始位置
if lost_target_frames >= LOST_TARGET_DELAY:
pwm_ud.duty_ns(tilt_angle_to_ns(TILT_ANGLE_INIT))
target_history = []
# 绘制中心十字
img.draw_cross(CX, CY, color=(0,255,0), size=15, thickness=3)
# 显示状态
status = "跟踪中" if blobs else "等待目标"
color = (0,255,255) if blobs else (255,100,100)
img.draw_string_advanced(20, 40, 32, status, color=color)
# FPS统计
frame_count += 1
if frame_count % 30 == 0:
current_time = time.ticks_ms()
elapsed = current_time - last_frame_time
fps = 30000 // max(1, elapsed)
last_frame_time = current_time
img.draw_string_advanced(W-100, H-40, 22, f"FPS: {fps}", color=(100,255,100))
# 显示图像
Display.show_image(img)
# 控制帧率(30FPS)
frame_time = time.ticks_diff(time.ticks_ms(), frame_start)
if frame_time < 33:
time.sleep_ms(33 - frame_time)
# 重置错误计数(成功处理一帧)
error_count = 0
except Exception as frame_error:
# 单帧错误不立即退出
error_count += 1
print(f"\n帧{frame_count}处理错误[{error_count}/{max_errors}]: {frame_error}")
if error_count >= max_errors:
print(f"\n连续{max_errors}帧错误,程序退出")
raise # 抛出异常退出主循环
time.sleep_ms(100) # 短暂延迟后重试
except KeyboardInterrupt:
print("\n用户中断...")
except Exception as e:
print(f"\n!!! 发生错误 !!!")
print(f"错误类型: {type(e).__name__}")
print(f"错误信息: {e}")
print(f"错误帧数: {frame_count}")
import sys
sys.print_exception(e)
finally:
print("\n清理资源...")
pwm_lr.duty_ns(SERVO_MID_NS)
pwm_ud.duty_ns(tilt_angle_to_ns(TILT_ANGLE_INIT))
time.sleep(1)
print(f"总共处理 {frame_count} 帧")
print("系统停止")
if __name__ == "__main__":
main()都可以实现追踪物体，并且显示距离远近（通过对像素的识别，主要是为了给下位机传信号做准备）
效果展示
视频我在后续文章会发布，等我研究一下。
最后一语
这个学期我会比较忙，所以csdn我不会更新的太频繁，大家见谅。
我给你一个从未有过信仰的人的忠诚。
我给你我设法保全的我自己的核心
------不营字造句，不和梦交易，不被时间，欢乐和逆境触动的核心。
博尔赫斯/《我用什么才能留住你》
感谢观看，共勉！！