import time
import os
import sys
from media.sensor import *
from media.display import *
from media.media import *
from time import ticks_ms
from machine import FPIOA
from machine import Pin
from machine import UART
sensor = None
# PID控制器类 - 参数优化以提高响应速度
class PIDController:
def __init__(self, kp, ki, kd, setpoint):
self.kp = kp
self.ki = ki
self.kd = kd
self.setpoint = setpoint
self.prev_error = 0
self.integral = 0
self.last_output = 0
def compute(self, current_value):
error = self.setpoint - current_value
# P项 - 增加比例增益
p_term = self.kp * error
# I项 - 限制积分项防止过冲
self.integral += error
i_term = self.ki * self.integral
# 积分限幅
i_term = max(-50, min(50, i_term))
# D项 - 增加微分增益
derivative = error - self.prev_error
d_term = self.kd * derivative
# 更新前次误差
self.prev_error = error
# 计算输出
output = p_term + i_term + d_term
# 限制输出范围
output = max(-100, min(100, output))
# 应用输出变化率限制
max_change = 30 # 最大输出变化率
if abs(output - self.last_output) > max_change:
if output > self.last_output:
output = self.last_output + max_change
else:
output = self.last_output - max_change
self.last_output = output
return output
# 通信协议定义
HEADER = 0xAA
FOOTER = 0x55
# 创建步进电机控制命令
def create_stepper_command(axis, direction, steps):
"""
创建步进电机控制命令
axis: 0-X轴, 1-Y轴
direction: 0-正转, 1-反转
steps: 步数 (0-65535)
"""
cmd = bytearray(6)
cmd[0] = HEADER
cmd[1] = 0x01 # 命令类型: 步进电机控制
cmd[2] = (axis << 4) | direction
cmd[3] = (steps >> 8) & 0xFF # 高字节
cmd[4] = steps & 0xFF # 低字节
cmd[5] = FOOTER
return cmd
# 发送云台控制命令 - 增加响应速度
def send_stepper_command(uart, axis, direction, steps):
# 限制最小步数,避免微小的抖动
if steps < 5:
return
cmd = create_stepper_command(axis, direction, steps)
uart.write(cmd)
# print(f"发送命令: X={axis}, 方向={direction}, 步数={steps}")
try:
# 1. 初始化显示和媒体管理器
print("初始化显示...")
Display.init(Display.ST7701, to_ide=True, width=800, height=480)
MediaManager.init()
print("显示初始化完成")
# 2. 初始化UART通信
print("初始化UART...")
fpioa = FPIOA()
fpioa.set_function(11, FPIOA.UART2_TXD)
fpioa.set_function(12, FPIOA.UART2_RXD)
uart = UART(UART.UART2, 115200, 8, 0, 1, timeout=1000)
print("UART初始化完成")
# 3. 初始化摄像头
print("初始化摄像头...")
sensor = Sensor(width=640, height=480)
sensor.reset()
sensor.set_framesize(width=640, height=480)
sensor.set_pixformat(Sensor.RGB565)
# 4. 启动摄像头
print("启动摄像头...")
sensor.run()
print("摄像头启动完成")
# 5. 设置窗口 - 增大窗口尺寸
window_roi = (0, 0, 640, 480)
clock = time.clock()
# 颜色阈值
thr_write = (60, 100, -20, 20, -20, 20) # 白色区域阈值
thr_black = (0, 35, -20, 20, -20, 20) # 黑色边框阈值
# 状态变量
target_offset = [-1, -1] # 目标补偿量
target = [320, 240] # 目标坐标 (初始化为中心)
prev_target = [320, 240] # 上一次目标坐标
view_offset = [0, 0] # 视角偏移补偿量
deviation = [0, 0] # 最终输出结果
# 创建PID控制器 - 参数优化
pid_x = PIDController(kp=0.8, ki=0.005, kd=0.2, setpoint=320) # 增加Kp, 减少Ki
pid_y = PIDController(kp=0.8, ki=0.005, kd=0.2, setpoint=240) # 增加Kp, 减少Ki
# 目标丢失计数器
target_lost_count = 0
MAX_TARGET_LOST = 10 # 允许连续丢失的最大帧数
# 绘制UI元素
def draw_ui(img):
"""绘制用户界面元素"""
# 绘制中心十字线
img.draw_line(0, 240, 640, 240, color=(0, 255, 0))
img.draw_line(320, 0, 320, 480, color=(0, 255, 0))
# 绘制目标点
img.draw_cross(int(target[0]), int(target[1]), color=(255, 0, 0), size=20, thickness=2)
# 绘制偏差指示器
img.draw_circle(320 + int(deviation[0]), 240 + int(deviation[1]), 5,
color=(255, 0, 0), thickness=2, fill=True)
# 显示帧率
fps = clock.fps()
img.draw_string(10, 10, f"FPS: {fps:.1f}", color=(255, 0, 0))
# 显示控制信息
img.draw_string(10, 30, f"Dev X: {deviation[0]:.1f}, Y: {deviation[1]:.1f}", color=(255, 0, 0))
# 显示通信状态
img.draw_string(10, 50, "MSPM0G3507 Connected", color=(0, 255, 0))
# 显示目标状态
status = "Target Found" if target_lost_count < MAX_TARGET_LOST else "Target Lost"
color = (0, 255, 0) if status == "Target Found" else (255, 0, 0)
img.draw_string(10, 70, f"Status: {status}", color=color)
print("进入主循环...")
# 主循环
while True:
clock.tick()
os.exitpoint()
start_time = ticks_ms()
# 获取图像
img = sensor.snapshot(chn=CAM_CHN_ID_0)
# 使用完整图像,不再裁剪
# img = img.copy(roi=window_roi)
# 寻找黑色边框 - 优化搜索参数
black_blobs = img.find_blobs([thr_black], pixels_threshold=200, area_threshold=200)
target_found = False
if black_blobs: # 如果找到黑色边框
for single_black_blob in black_blobs:
# 检查黑色区域的填充率
fill_ratio = single_black_blob.pixels() / (single_black_blob.w() * single_black_blob.h())
if fill_ratio < 0.3: # 填充率低的区域可能是边框
# 在黑色区域内寻找白色区域
write_blobs = img.find_blobs([thr_write], roi=single_black_blob.rect(),
pixels_threshold=50, area_threshold=2)
if write_blobs: # 如果找到白色区域
# 找到最大的白色区域
largest_white = max(write_blobs, key=lambda b: b.area())
# 检查白色区域与黑色区域的关系
white_black_ratio = largest_white.pixels() / single_black_blob.pixels()
center_diff_x = abs(largest_white.cx() - single_black_blob.cx())
center_diff_y = abs(largest_white.cy() - single_black_blob.cy())
# 验证是否满足目标条件
if (2 < white_black_ratio < 4) and center_diff_x < 20 and center_diff_y < 20:
# 计算目标位置
target[0] = largest_white.cx() + target_offset[0]
target[1] = largest_white.cy() + target_offset[1]
target_found = True
target_lost_count = 0 # 重置丢失计数器
# 在目标周围绘制矩形
img.draw_rectangle(single_black_blob.rect(), color=(0, 255, 0), thickness=2)
img.draw_rectangle(largest_white.rect(), color=(255, 0, 0), thickness=2)
break # 找到目标后退出循环
# 目标丢失处理
if not target_found:
target_lost_count += 1
if target_lost_count > MAX_TARGET_LOST:
# 长时间丢失目标,重置位置到中心
target[0] = 320
target[1] = 240
# 重置PID积分项
pid_x.integral = 0
pid_y.integral = 0
else:
# 短期丢失,使用上一次的位置
target[0] = prev_target[0]
target[1] = prev_target[1]
else:
prev_target[0] = target[0]
prev_target[1] = target[1]
# 更新视角偏移 - 增加更新速度
view_offset[0] += round((target[0] - 320) * 0.4) # 从0.2增加到0.4
view_offset[1] += round((target[1] - 240) * 0.4) # 从0.2增加到0.4
# 限制视角偏移范围
view_offset[0] = min(300, max(0, view_offset[0]))
view_offset[1] = min(200, max(0, view_offset[1]))
# 计算最终偏差
deviation[0] = target[0] + view_offset[0] - 320
deviation[1] = target[1] + view_offset[1] - 240
# 使用PID计算云台控制量
control_x = pid_x.compute(target[0])
control_y = pid_y.compute(target[1])
# 转换为步进电机控制命令
# 减小死区阈值,增加灵敏度
if abs(control_x) > 3: # 死区控制 (从5减小到3)
direction_x = 0 if control_x > 0 else 1
steps_x = int(abs(control_x) * 8) # 增加比例因子 (从5增加到8)
send_stepper_command(uart, axis=0, direction=direction_x, steps=steps_x)
if abs(control_y) > 3: # 死区控制 (从5减小到3)
direction_y = 0 if control_y > 0 else 1
steps_y = int(abs(control_y) * 8) # 增加比例因子 (从5增加到8)
send_stepper_command(uart, axis=1, direction=direction_y, steps=steps_y)
# 更新窗口位置
# new_x = 320 + view_offset[0]
# new_y = 240 + view_offset[1]
# window_roi = (new_x, new_y, 240, 240)
# 绘制UI
draw_ui(img)
# 显示图像 - 简化显示流程
try:
# 压缩图像用于显示
img.compress_for_ide()
# 显示图像
Display.show_image(img, x=(800-img.width())//2, y=(480-img.height())//2)
except Exception as e:
print(f"显示错误: {e}")
# 尝试重新初始化显示
try:
Display.deinit()
Display.init(Display.ST7701, to_ide=True, width=800, height=480)
print("显示重新初始化成功")
except Exception as e2:
print(f"显示重新初始化失败: {e2}")
# 计算处理时间
process_time = ticks_ms() - start_time
fps = clock.fps()
if process_time > 0:
real_fps = 1000 / process_time
print(f"用时: {process_time}ms, 实时帧速: {real_fps:.1f}fps, 平均帧速: {fps:.1f}fps")
else:
print(f"用时: {process_time}ms, 平均帧速: {fps:.1f}fps")
# 接收来自MSPM0G3507的响应
if uart.any():
response = uart.read()
print(f"收到响应: {response}")
except KeyboardInterrupt as e:
print("用户停止: ", e)
except Exception as e:
print(f"异常: {e}")
# 打印异常详细信息
import sys
sys.print_exception(e)
finally:
print("清理资源...")
if 'sensor' in globals() and isinstance(sensor, Sensor):
try:
sensor.stop()
except:
pass
if 'Display' in globals():
try:
Display.deinit()
except:
pass
os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
time.sleep_ms(100)
if 'MediaManager' in globals():
try:
MediaManager.deinit()
except:
pass
print("程序退出")
这是K230的代码,但他的反应速度太慢了,刚开始的帧率很低,帮我修改解决一下