import sensor, image, time # 导入摄像头、图像处理、时间模块
from pyb import Pin, LED, Timer # 导入Pyboard的引脚、LED和定时器模块
from machine import UART # 导入UART串口模块
uart_A = UART(3, 115200) # 创建UART对象,使用串口3,波特率115200
uart_A.init(115200, bits=8, parity=None, stop=1) # 初始化串口参数,8位数据,无校验,1位停止位
sensor.reset() # 初始化摄像头传感器
sensor.set_pixformat(sensor.RGB565) # 设置摄像头像素格式为RGB565
sensor.set_framesize(sensor.QVGA) # 设置摄像头分辨率为QVGA(320x240)
sensor.skip_frames(time=333) # 跳过333ms的帧,等待画面稳定
sensor.set_auto_gain(False) # 关闭自动增益
sensor.set_auto_whitebal(False) # 关闭自动白平衡
pin1 = Pin('P0', Pin.IN, Pin.PULL_DOWN) # 初始化P0引脚为输入,下拉模式
EXPOSURE_TIME_SCALE = 1 # 曝光时间缩放因子
print("Initial exposure == %d" % sensor.get_exposure_us()) # 打印初始曝光时间
sensor.set_auto_gain(False) # 再次确保关闭自动增益
sensor.set_auto_whitebal(False) # 再次确保关闭自动白平衡
sensor.skip_frames(time=1000) # 跳过1000ms的帧,进一步稳定画面
current_exposure_time_in_microseconds = sensor.get_exposure_us() # 再次获取当前曝光时间
print("Current Exposure == %d" % current_exposure_time_in_microseconds) # 打印当前曝光时间
sensor.set_auto_exposure(False, exposure_us=int(current_exposure_time_in_microseconds * EXPOSURE_TIME_SCALE)) # 设置为手动曝光
red_led = LED(1) # 获取板载红色LED对象,编号为1
roi2 = [0, 0, 320, 240] # 设置ROI(感兴趣区域)为整幅画面
thresholds = (0, 14, -128, 127, -128, 127) # 非红色目标的颜色阈值(用于找最大斑点)
red_threshold = (93, 100, -128, 127, -128, 127) # 红色目标的颜色阈值
# 各种任务和点标志位,初始为0,表示未完成
TASK1_OK = 0
TASK2_OK = 0
TASK3_OK = 0
TASK4_OK = 0
top_left_OK = 0
top_mid_OK = 0
top_right_OK = 0
right_mid_OK = 0
bottom_left_OK = 0
bottom_mid_OK = 0
bottom_right_OK = 0
left_mid_OK = 0
# 舵机相关变量
pan_step = 1 # 水平增量步长
tilt_step = 1 # 垂直增量步长
last_pan = 0 # 上一次水平误差
last_tilt = 0 # 上一次垂直误差
set_pan = 0 # 当前水平PWM值
set_tilt = 0 # 当前垂直PWM值
jiaodian = 0 # 是否已获取了边框顶点(焦点),0为否,1为是
ledflag = 0 # 控制LED闪烁的次数
red_led.off() # 关闭红色LED
largest_blob = None # 储存最大斑点对象
max_blob_area = 0 # 最大斑点面积
red_blobs = 0 # 红色斑点数量
def angle_to_pwm(angle): # 角度转PWM脉宽
angle_min = 0 # 最小角度
angle_max = 180 # 最大角度
pwm_min = 500 # 最小PWM
pwm_max = 2500 # 最大PWM
pwm = int((pwm_max - pwm_min) / (angle_max - angle_min) * angle + pwm_min) # 线性转换
pwm = int(pwm / 1) * 1 # 保证为整数
return pwm
def SERVO_task(x, y): # 控制舵机移动到指定位置(用较慢速度)
SERVO_data = "{{#000P{:04d}T0500!#001P{:04d}T0500!}}".format(x, y) # 构造舵机指令字符串
for i in range(4): # 连续发送4次,提升可靠性
uart_A.write(SERVO_data)
time.sleep_ms(2) # 小延时
def SERVO_task3(x, y): # 控制舵机快速移动到指定位置
x = int(x / 1) * 1 # 转为整数
y = int(y / 1) * 1 # 转为整数
SERVO_data = "{{#000P{:04d}T0001!#001P{:04d}T0001!}}".format(x, y) # 舵机指令(最快速度)
uart_A.write(SERVO_data) # 发送指令
def LED_task(ledflag): # 控制LED闪烁ledflag次
for l in range(0, ledflag, 1):
red_led.on()
time.sleep_ms(200) # 亮0.2秒
red_led.off()
time.sleep_ms(200) # 灭0.2秒
def find_box_corners(edges): # 从边界点中找出四个角
if len(edges) < 1:
return None
top_left = edges[0] # 初始化四角
top_right = edges[0]
bottom_left = edges[0]
bottom_right = edges[0]
for edge in edges: # 遍历所有点,按规则找出四角
x, y = edge
if x + y < top_left[0] + top_left[1]:
top_left = edge
if x - y > top_right[0] - top_right[1]:
top_right = edge
if x - y < bottom_left[0] - bottom_left[1]:
bottom_left = edge
if x + y > bottom_right[0] + bottom_right[1]:
bottom_right = edge
# 坐标微调,优化顶点位置,避免检测误差
if top_left[1] < top_right[1] and top_left[1] < bottom_left[1] and top_left[1] < bottom_right[1] and abs(top_left[1] - top_right[1]) > 15:
top_left = top_left[0] + 2, top_left[1] + 2
bottom_right = bottom_right[0] - 2, bottom_right[1] - 2
elif top_right[1] < top_left[1] and top_right[1] < bottom_left[1] and top_right[1] < bottom_right[1] and abs(top_left[1] - top_right[1]) > 15:
top_right = top_right[0] - 2, top_right[1] + 2
top_left = top_left[0] + 2, top_left[1]
bottom_left = bottom_left[0] + 2, bottom_left[1] - 2
else:
top_right = top_right[0] - 3, top_right[1] + 3
top_left = top_left[0] + 3, top_left[1] + 3
bottom_right = bottom_right[0] - 3, bottom_right[1] - 3
bottom_left = bottom_left[0] + 3, bottom_left[1] - 3
return top_left, top_right, bottom_left, bottom_right
def pan_can_angle(pan_servo_angle): # 限定水平舵机角度在安全范围
if pan_servo_angle <= 77:
pan_servo_angle = 77
if pan_servo_angle >= 95:
pan_servo_angle = 95
return pan_servo_angle
def tilt_can_angle(tilt_servo_angle): # 限定垂直舵机角度在安全范围
if tilt_servo_angle <= 72:
tilt_servo_angle = 72
if tilt_servo_angle >= 90:
tilt_servo_angle = 90
return tilt_servo_angle
TASK = 0 # 当前任务编号,0为无任务
time.sleep_ms(200) # 启动延时200ms
print(TASK) # 打印初始任务编号
while True: # 主循环
# 1.根据P0状态选择任务
if pin1.value() == 1: # 如果外部按键或信号触发
TASK = 2
LED_task(TASK) # LED闪烁对应次数
print(TASK)
else:
TASK = 1
LED_task(TASK)
print(TASK)
# 2.任务未完成时初始化舵机位置
if TASK != 0 and TASK1_OK != 1 and TASK2_OK != 1 and TASK3_OK != 1 and TASK4_OK != 1:
pan_angle = 90 # 水平中位
tilt_angle = 90 # 垂直中位
# 3.根据任务编号处理
if TASK == 0 or (TASK == 1 and TASK1_OK == 1) or (TASK == 2 and TASK2_OK == 1):
img = sensor.snapshot() # 仅采集图像
elif TASK == 1 and TASK1_OK == 0:
SERVO_task(angle_to_pwm(pan_angle - 2), angle_to_pwm(tilt_angle - 9)) # 舵机移动到指定位置
TASK1_OK = 1 # 标志任务1已完成
elif TASK == 2 and TASK2_OK == 0:
# 执行一系列舵机移动,实现某种轨迹
SERVO_task(angle_to_pwm(pan_angle + 10), angle_to_pwm(tilt_angle - 18))
time.sleep_ms(1200)
SERVO_task(angle_to_pwm(pan_angle - 12), angle_to_pwm(tilt_angle - 18))
time.sleep_ms(1200)
SERVO_task(angle_to_pwm(pan_angle - 12), angle_to_pwm(tilt_angle + 1))
time.sleep_ms(1200)
SERVO_task(angle_to_pwm(pan_angle + 10), angle_to_pwm(tilt_angle - 1))
time.sleep_ms(1200)
SERVO_task(angle_to_pwm(pan_angle + 10), angle_to_pwm(tilt_angle - 18))
TASK2_OK = 1 # 标志任务2已完成
elif (TASK == 3 and TASK3_OK == 0) or (TASK == 4 and TASK4_OK == 0):
# 进入检测与跟踪流程
SERVO_task(angle_to_pwm(pan_angle + 10), angle_to_pwm(tilt_angle - 9))
time.sleep_ms(800)
while (TASK3_OK == 0 or TASK4_OK == 0): # 持续直到检测完毕
# 实时任务切换
if pin1.value() == 1 and TASK == 3:
TASK = 4 # 切换为任务4
TASK3_OK = 0
left_mid_OK = 0
top_left_OK = 0
top_mid_OK = 0
top_right_OK = 0
right_mid_OK = 0
bottom_left_OK = 0
bottom_mid_OK = 0
bottom_right_OK = 0
jiaodian = 0
LED_task(TASK)
break
if pin1.value() == 1 and TASK == 4:
TASK = 0 # 复位
time.sleep_ms(600)
LED_task(TASK)
break
img = sensor.snapshot() # 获取实时图像
if jiaodian == 0:
# 查找画面中的最大斑点,推测为目标框
blobs = img.find_blobs([thresholds], roi=roi2, pixels_threshold=0, area_threshold=0, merge=True)
for blob in blobs:
area = blob.area()
if area > max_blob_area:
largest_blob = blob
max_blob_area = area
if largest_blob:
edges = largest_blob.min_corners() # 获取最小外接矩形的角点
top_left, top_right, bottom_left, bottom_right = find_box_corners(edges)
img.draw_cross(top_left[0], top_left[1]) # 在图像上标记四个角
img.draw_cross(top_right[0], top_right[1])
img.draw_cross(bottom_left[0], bottom_left[1])
img.draw_cross(bottom_right[0], bottom_right[1])
up_w = top_right[0] - top_left[0] # 上边宽
up_h = top_left[1] - top_right[1] # 上边高(理论为0)
left_w = bottom_left[0] - top_left[0] # 左边宽(理论为0)
left_h = bottom_left[1] - top_left[1] # 左边高
print(up_w, up_h, left_w, left_h) # 打印框的几何信息
set_pan = angle_to_pwm(pan_angle + 10) # 设置舵机目标位置
set_tilt = angle_to_pwm(tilt_angle - 9)
jiaodian = 1 # 标记已获取焦点
time.sleep_ms(400)
# 查找红色斑点,进行坐标误差计算和舵机控制
red_blobs = img.find_blobs([red_threshold], roi=roi2, pixels_threshold=0, area_threshold=2, merge=True)
# === 顶点跟踪阶段1:左上点 ===
if TASK3_OK == 0 and ((jiaodian == 1 and top_left_OK == 0) or (jiaodian == 1 and top_left_OK == 1 and top_right_OK == 1 and bottom_right_OK == 1 and bottom_left_OK == 1)):
if red_blobs:
for red_blob in red_blobs:
img.draw_cross(red_blob.cx(), red_blob.cy()) # 标记红点
if left_mid_OK == 0 and bottom_left_OK == 1:
# 跟踪左中点
pan_error = red_blob.cx() - (top_left[0] + bottom_left[0]) / 2
tilt_error = red_blob.cy() - (top_left[1] + bottom_left[1]) / 2
else:
# 跟踪左上点
pan_error = red_blob.cx() - top_left[0]
tilt_error = red_blob.cy() - top_left[1]
# 误差归一化和限幅(防止过大或过小影响控制)
if pan_error != 0 and tilt_error != 0:
w_error = abs(pan_error / tilt_error)
h_error = abs(tilt_error / pan_error)
w_error = min(max(w_error, 0.2), 0.7)
h_error = min(max(h_error, 0.2), 0.7)
# 水平控制
if abs(pan_error - last_pan) < 3:
if pan_error > 2:
pan_step = w_error
elif pan_error < -2:
pan_step = -w_error
if abs(pan_error) < 8:
pan_step = 0
set_pan += pan_step
# 垂直控制
if abs(tilt_error - last_tilt) < 3:
if tilt_error > 2:
tilt_step = -h_error
elif tilt_error < -2:
tilt_step = h_error
if abs(tilt_error) < 8:
tilt_step = 0
set_tilt += tilt_step
last_pan = pan_error # 记录误差
last_tilt = tilt_error
tilt_step = 0
pan_step = 0
# 判定到位
if abs(pan_error) < 8 and abs(tilt_error) < 8:
if left_mid_OK == 0:
left_mid_OK = 1
else:
top_left_OK = 1
if bottom_left_OK == 1:
if TASK == 3:
TASK3_OK = 1
break
elif TASK == 4:
TASK3_OK = 0
left_mid_OK = 0
top_left_OK = 0
top_mid_OK = 0
top_right_OK = 0
right_mid_OK = 0
bottom_left_OK = 0
bottom_mid_OK = 0
bottom_right_OK = 0
SERVO_task3(int(set_pan), int(set_tilt)) # 实时发送舵机命令
# === 顶点跟踪阶段2:右上点 ===
if jiaodian == 1 and top_left_OK == 1 and top_right_OK == 0:
if red_blobs:
for red_blob in red_blobs:
img.draw_cross(red_blob.cx(), red_blob.cy())
if top_mid_OK == 0:
pan_error = red_blob.cx() - (top_right[0] + top_left[0]) / 2
tilt_error = red_blob.cy() - (top_right[1] + top_left[1]) / 2
else:
pan_error = red_blob.cx() - top_right[0]
tilt_error = red_blob.cy() - top_right[1]
if pan_error != 0 and tilt_error != 0:
w_error = abs(pan_error / tilt_error)
h_error = abs(tilt_error / pan_error)
w_error = min(max(w_error, 0.2), 0.7)
h_error = min(max(h_error, 0.2), 0.7)
if abs(pan_error - last_pan) < 3:
if pan_error > 2:
pan_step = w_error
elif pan_error < -2:
pan_step = -w_error
if abs(pan_error) < 8:
pan_step = 0
set_pan += pan_step
if abs(tilt_error - last_tilt) < 3:
if tilt_error > 2:
tilt_step = -h_error
elif tilt_error < -2:
tilt_step = h_error
if abs(tilt_error) < 8:
tilt_step = 0
set_tilt += tilt_step
print("pan_step=", pan_step, "tilt_step", tilt_step)
tilt_step = 0
pan_step = 0
last_pan = pan_error
last_tilt = tilt_error
if abs(pan_error) < 8 and abs(tilt_error) < 8:
if top_mid_OK == 0:
top_mid_OK = 1
else:
top_right_OK = 1
SERVO_task3(int(set_pan), int(set_tilt))
# === 顶点跟踪阶段3:右下点 ===
if jiaodian == 1 and top_left_OK == 1 and top_right_OK == 1 and bottom_right_OK == 0:
if red_blobs:
for red_blob in red_blobs:
img.draw_cross(red_blob.cx(), red_blob.cy())
if right_mid_OK == 0:
pan_error = red_blob.cx() - (top_right[0] + bottom_right[0]) / 2
tilt_error = red_blob.cy() - (top_right[1] + bottom_right[1]) / 2
else:
pan_error = red_blob.cx() - bottom_right[0]
tilt_error = red_blob.cy() - bottom_right[1]
if pan_error != 0 and tilt_error != 0:
w_error = abs(pan_error / tilt_error)
h_error = abs(tilt_error / pan_error)
w_error = min(max(w_error, 0.2), 0.7)
h_error = min(max(h_error, 0.2), 0.7)
if abs(pan_error - last_pan) < 3:
if pan_error > 2:
pan_step = w_error
elif pan_error < -2:
pan_step = -w_error
if abs(pan_error) < 8:
pan_step = 0
set_pan += pan_step
if abs(tilt_error - last_tilt) < 3:
if tilt_error > 1:
tilt_step = -h_error
elif tilt_error < -1:
tilt_step = h_error
if abs(tilt_error) < 8:
tilt_step = 0
set_tilt += tilt_step
print("pan_step=", pan_step, "tilt_step", tilt_step)
tilt_step = 0
pan_step = 0
last_pan = pan_error
last_tilt = tilt_error
if abs(pan_error) < 8 and abs(tilt_error) < 8:
if right_mid_OK == 0:
right_mid_OK = 1
else:
bottom_right_OK = 1
SERVO_task3(int(set_pan), int(set_tilt))
# === 顶点跟踪阶段4:左下点 ===
if jiaodian == 1 and top_left_OK == 1 and top_right_OK == 1 and bottom_right_OK == 1 and bottom_left_OK == 0:
if red_blobs:
for red_blob in red_blobs:
img.draw_cross(red_blob.cx(), red_blob.cy())
if bottom_mid_OK == 0:
pan_error = red_blob.cx() - (bottom_left[0] + bottom_right[0]) / 2
tilt_error = red_blob.cy() - (bottom_left[1] + bottom_right[1]) / 2
else:
pan_error = red_blob.cx() - bottom_left[0]
tilt_error = red_blob.cy() - bottom_left[1]
if pan_error != 0 and tilt_error != 0:
w_error = abs(pan_error / tilt_error)
h_error = abs(tilt_error / pan_error)
w_error = min(max(w_error, 0.2), 0.7)
h_error = min(max(h_error, 0.2), 0.7)
if abs(pan_error - last_pan) < 3:
if pan_error > 2:
pan_step = w_error
elif pan_error < -2:
pan_step = -w_error
if abs(pan_error) < 8:
pan_step = 0
set_pan += pan_step
if abs(tilt_error - last_tilt) < 3:
if tilt_error > 2:
tilt_step = -h_error
elif tilt_error < -2:
tilt_step = h_error
if abs(tilt_error) < 8:
tilt_step = 0
set_tilt += tilt_step
print("pan_step=", pan_step, "tilt_step", tilt_step)
tilt_step = 0
pan_step = 0
last_pan = pan_error
last_tilt = tilt_error
if abs(pan_error) < 8 and abs(tilt_error) < 8:
if bottom_mid_OK == 0:
bottom_mid_OK = 1
else:
bottom_left_OK = 1
left_mid_OK = 0
SERVO_task3(int(set_pan), int(set_tilt))这是main.py
import time, image,sensor,math,pyb,ustruct, struct
from image import SEARCH_EX, SEARCH_DS
from pyb import Pin, Timer,LED
sensor.reset()
sensor.set_contrast(1)
sensor.set_gainceiling(16)
sensor.set_framesize(sensor.QQVGA)
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_windowing((0, 50, 160, 50))
EXPOSURE_TIME_SCALE = 1
print("Initial exposure == %d" % sensor.get_exposure_us())
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.skip_frames(time = 1000)
current_exposure_time_in_microseconds = sensor.get_exposure_us()
print("Current Exposure == %d" % current_exposure_time_in_microseconds)
sensor.set_auto_exposure(False, \
exposure_us = int(current_exposure_time_in_microseconds * EXPOSURE_TIME_SCALE))
uart = pyb.UART(3, 115200, timeout_char = 1000)
data = [0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00]
def servo_control(servo_ID):
img_data_0 = "#000" + "PID00" + str(servo_ID) + "!"
uart.write(img_data_0)
print(img_data_0)
time.sleep_ms(10)
while(True):
servo_control(0) #在这里写上要修改的ID号(下面控制左右摆动的设置舵机ID:000,上面控制上下的舵机设置ID:001)
time.sleep_ms(1000)#servo_control(1)这样就是设置ID为1,记得设置舵机时 只连接一个舵机,别串连,不然两个舵机都设置成了同一个ID
这是SetID.py
将这个代码改成openmv控制两个舵机,进行图像识别,识别一个矩形的中心点,将舵机的正中心对准矩形的中心点,矩形四周用黑胶带做边框