# Find Rects Example
#
# This example shows off how to find rectangles in the image using the quad threshold
# detection code from our April Tags code. The quad threshold detection algorithm
# detects rectangles in an extremely robust way and is much better than Hough
# Transform based methods. For example, it can still detect rectangles even when lens
# distortion causes those rectangles to look bent. Rounded rectangles are no problem!
# (But, given this the code will also detect small radius circles too)...
import time, os, gc, sys
from media.sensor import *
from media.display import *
from media.media import *
from machine import UART
import time
from machine import PWM, FPIOA
#servo
def find_majority_element(nums):
"""
查找数组中出现次数超过一半的元素(众数)
参数:
nums (list): 整数数组
返回:
int or None: 众数(如果存在),否则返回None
"""
candidate = None # 候选众数
count = 0 # 计数器
# 第一轮遍历:找出候选众数
for num in nums:
if count == 0:
candidate = num # 当计数器为0时,更新候选众数
count += (1 if num == candidate else -1) # 相同则加1,不同则减1
# 验证候选众数是否确实超过半数
count = 0
for num in nums:
if num == candidate:
count += 1
# 如果出现次数超过数组长度的一半,则返回候选众数
return candidate if count > len(nums) // 2 else None
#pid
class PID:
def __init__(self, kp, ki, input_value, target=320):
self.e = 0
self.e_last = 0
self.kp = kp
self.ki = ki
self.target = target
self.input_value = input_value
def cal(self, value):
self.e = self.target - value
delta = self.kp * (self.e-self.e_last) + self.ki * self.e
self.e_last = self.e
self.input_value = self.input_value + delta
return self.input_value
c_x=160
c_y=120
pid_x = PID(-0.002, -0.0003, 1.5/20*100, c_x)
# 垂直控制PID: 比例系数-0.002, 积分系数-0.0003, 初始控制值1.5ms
pid_y = PID(-0.002, -0.0003, 0.5/20*100, c_y)
#image
DETECT_WIDTH = ALIGN_UP(320, 16)
DETECT_HEIGHT = 240
sensor = None
color_threshold =(71, 100, -15, 15, -34, -3)
#color_threshold =((97, 100, -14, -4, -45, -2))
def camera_init():
global sensor
# construct a Sensor object with default configure
sensor = Sensor(width=DETECT_WIDTH,height=DETECT_HEIGHT)
# sensor reset
sensor.reset()
# set hmirror
# sensor.set_hmirror(False)
# sensor vflip
# sensor.set_vflip(False)
# set chn0 output size
sensor.set_framesize(width=DETECT_WIDTH,height=DETECT_HEIGHT)
# set chn0 output format
sensor.set_pixformat(Sensor.RGB565)
# use IDE as display output
Display.init(Display.VIRT, width= DETECT_WIDTH, height = DETECT_HEIGHT,fps=100,to_ide = True)
# init media manager
MediaManager.init()
# sensor start run
sensor.run()
#######################image_function
def camera_deinit():
global sensor
# sensor stop run
sensor.stop()
# deinit display
Display.deinit()
# sleep
os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
time.sleep_ms(100)
# release media buffer
MediaManager.deinit()
def find_min_b(blobs):
min_size=1000000
for blob in blobs:
if min_size > blob[2]*blob[3] :
min_blob=blob
min_size = blob[2]*blob[3]
return min_blob
def find_max_c(circles):
max_size=0
for circle in circles:
if circle[3] > max_size:
max_circle=circle
max_size = circle[3]
return max_circle
def find_max_r(rects):
max_size=0
for rect in rects:
if rect[2]*rect[3] > max_size:
max_rect=rect
max_size = rect[2]*rect[3]
return max_rect
#############################servo_function
def pwm_init():
pwm_io1 = FPIOA()
pwm_io1.set_function(47, FPIOA.PWM3)
pwm_shang = PWM(3, 50, 50, enable=True) # 配置PWM3,默认频率50Hz,占空比50%,xia
# 配置排针引脚号32,芯片引脚号为46的排针复用为PWM通道2输出
pwm_io2 = FPIOA()
pwm_io2.set_function(46, FPIOA.PWM2)
pwm_xia = PWM(2, 50, 50, enable=True) # 配置PWM2,默认频率50Hz,占空比50%,shang
pwm_xia.duty(7.5) #xia舵机旋转到中间
pwm_shang.duty(2.5) #shang舵机旋转到中间
def capture_picture():
os.exitpoint()
global sensor
img = sensor.snapshot()
# `threshold` below should be set to a high enough value to filter out noise
# rectangles detected in the image which have low edge magnitudes. Rectangles
# have larger edge magnitudes the larger and more contrasty they are...
for c in img.find_circles(threshold = 10000):
#img.draw_circle([v for v in r.circle()], color = (255, 0, 0))
img.draw_circle(c.x(), c.y(), c.r(), color=(255,0,0))
#for p in r.corners(): img.draw_circle(p[0], p[1], 5, color = (0, 255, 0))
img.draw_cross(c.x(), c.y(),color = (255, 0, 0))
print(c)
#for r in img.find_rects(threshold = 10000):
# img.draw_rectangle([v for v in r.rect()], color = (255, 255, 255))
# #for p in r.corners(): img.draw_circle(p[0], p[1], 5, color = (0, 255, 0))
# print(r)
#img_rect = img.to_grayscale(copy=True)
#img_rect = img_rect.binary([(23, 62)])
#rects=img_rect.find_rects(threshold =2000)
blobs=None
global flag
#global max_blob
global max_rect_
#max_blob=None
#max_rect_=None
#if rects:
# max_rect_= find_max_r(rects)
# img.draw_rectangle([v for v in max_rect_.rect()], color = (255, 255, 255))
# blobs = img.find_blobs([color_threshold],roi=max_rect_.rect(),merge=True,pixels_threshold=2,area_threshold=2)
for i in range(6):
img_rect = img.to_grayscale(copy=True)
img_rect = img_rect.binary([(23, 62)])
rects = img_rect.find_rects(threshold=10000) # 最小面积阈值10000像
# 绘制找到的矩形
if not rects == None:
for rect in rects:
corner = rect.corners() # 获取矩形四个角点
# 绘制矩形边界
img.draw_line(corner[0][0], corner[0][1], corner[1][0], corner[1][1], color=(0, 255, 0), thickness=1)
img.draw_line(corner[2][0], corner[2][1], corner[1][0], corner[1][1], color=(0, 255, 0), thickness=1)
img.draw_line(corner[2][0], corner[2][1], corner[3][0], corner[3][1], color=(0, 255, 0), thickness=1)
img.draw_line(corner[0][0], corner[0][1], corner[3][0], corner[3][1], color=(0, 255, 0), thickness=1)
# 计算矩形中心点
c_x = sum([corner[k][0] for k in range(4)])/4
c_y = sum([corner[k][1] for k in range(4)])/4
if len(rects) == 2:
print("center_point: {}".format([round(c_x), round(c_y)]))
flag = 1 # 进入追踪模式
pid_x.target = c_x
pid_y.target = c_y
max_rect_= find_max_r(rects)
t=0
t=t+1
blobs = img.find_blobs([color_threshold],roi=max_rect_.rect(),merge=True,pixels_threshold=2,area_threshold=2)
#else flag=0
if blobs:
max_blob = find_min_b(blobs)#调用函数,返回最大色块
#img.draw_circle(80,60,5,color=200)
img.draw_circle(max_blob.cx(),max_blob.cy(),10,color=200)
img.draw_rectangle((max_blob.x(),max_blob.y(),max_blob.w(),max_blob.h()),color=(255,0,0))#用红色框出最大色块
img.draw_string_advanced(0,0, "(x,y) =")
img.draw_string_advanced(max_blob.x()+40,max_blob.y(), str(max_blob.cx()))
img.draw_string_advanced(max_blob.x()+60,max_blob.y(), str(max_blob.cy()))#在框图显示色块的中心坐标
img.draw_string_advanced(40,0, str(max_blob.cx()))
img.draw_string_advanced(60,0, str(max_blob.cy()))#在框图左上角显示色块的中心坐标
c_x = max_blob.x() + max_blob.w() / 2
c_y = max_blob.y() + max_blob.h() / 2
new_duty = pid_x.cal(c_x) # 计算新占空比
# 限制占空比在0.5ms-2.5ms范围内(舵机安全范围)
new_duty = max(min(new_duty, 2.5/20*100), 0.5/20*100)
# 更新水平舵机位置
pwm_xia.enable(0) # 临时禁用PWM
pwm_xia.duty(round(new_duty, 2)) # 设置新占空比
pwm_xia.enable(1) # 重新启用PWM
# 垂直方向PID控制
new_duty = pid_y.cal(c_y) # 计算新占空比
# 限制占空比在安全范围内
new_duty = max(min(new_duty, 2.5/20*100), 0.5/20*100)
# 更新垂直舵机位置
pwm_shang.enable(0) # 临时禁用PWM
pwm_shang.duty(round(new_duty, 2)) # 设置新占空比
print(round(new_duty, 1)) # 调试输出
pwm_shang.enable(1) # 重新启用PWM
# draw result to screen
Display.show_image(img)
img = None
gc.collect()
def main():
os.exitpoint(os.EXITPOINT_ENABLE)
camera_is_init = False
fps = time.clock()
pwm_init()
try:
print("camera init")
camera_init()
camera_is_init = True
print("camera capture")
while True:
fps.tick()
try:
capture_picture()
except KeyboardInterrupt as e:
print("user stop: ", e)
break
except BaseException as e:
print(f"Exception {e}")
break
print(fps.fps())
except Exception as e:
print(f"Exception {e}")
finally:
if camera_is_init:
print("camera deinit")
camera_deinit()
if __name__ == "__main__":
main()
这段代码报错Exception Not enough positional arguments!
最新发布