import sensor, image, time,mathfrom pyb import Pin#from pid import PIDfrom pyb import Servofrom pyb import LED#红追绿，由于红色激光照到黑胶布上，红色激光无法识别。而绿色的可以，#因此我们使用绿色作为移动目标，红色作为跟踪。#本舵机使用红色激光笔，去追踪绿设激光点。#舵机结构红色激光笔与镜头一体，同时运动#红色中心点初始位置为 170，110，这个根据自己激光笔和摄像头固定的位置测量得到，#有可能存在误差red_x=170red_y=110pan_servo=Servo(1)tilt_servo=Servo(2)p_in0 = Pin('P0', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻，复位用p_in1 = Pin('P1', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻，停止光标移动p_in2 = Pin('P2', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻，重启光标移动#如果单纯实现追踪，可忽略以上设置。pan_servo.calibration(500,2500,500)tilt_servo.calibration(500,2500,500)time.sleep_ms(300)gray_threshold = (0, 191)#灰度阈值根据实际情况可微调，排除最亮的点即可，后续程序有取反green_threshold = (100, 78, -32, 91, -60, 99)#此处LAB阈值红绿通用，不改程序名称了sensor.reset() # Initialize the camera sensor.#sensor.set_pixformat(sensor.RGB565) # use RGB565.sensor.set_pixformat(sensor.GRAYSCALE) # use GRAYSCALE.sensor.set_framesize(sensor.QVGA) # use QVGA for speed.sensor.skip_frames(10) # Let new settings take affect.sensor.set_auto_whitebal(False) # turn this off.clock = time.clock() # Tracks FPS.while(True): # 但由于安装不够稳固，每次安装会有少许变化，需要在程序中进行修正。red_x,red_y # 跟踪过程实际上是用修正后的红色坐标去追踪绿色目标 ss=[[0,0],[0,0],[0,0]] #存储识别到的亮点坐标，实际上只需要两个，防止溢出多开一个空间 min_size=400 for i in range (1000): if p_in1.value()==0: #暂停mov_pwm while(True): if p_in2.value()==0: #恢复mov_pwm break j=0 r=[900,900] #初始化两个亮点到标准绿点的距离为一个比较大的值 img = sensor.snapshot() #blobs = img.find_blobs([green_threshold],x_stride=1,y_stride=1) blobs = img.find_blobs([gray_threshold],x_stride=1,y_stride=1,invert=True) if i > 10 : for blob in blobs: #尽量过滤掉一些杂散的blobs if (blob[2]blob[3] < min_size) and (blob[2]>2) and (blob[3]>2) and (blob[2]<2blob[3]) and (blob[3]<2blob[2]):#确保色块较小且较方（圆） yuandian = blob img.draw_rectangle(yuandian.rect(),color = (0,0,0)) # rect img.draw_cross(yuandian.cx(), yuandian.cy()) # cx, cyj v print(yuandian.cx(), yuandian.cy(),blob[2],blob[3]) if j>1: #蓝色LED点亮，表示搜索到3个以上的目标，设备不能正常跟踪 LED(3).on() #点亮蓝灯 time.sleep_ms(500) #LED(3).off() ss=[[0,0],[0,0],[0,0]] break else: #逐个记录识别到的激光点 ss[j][0]=yuandian.cx() ss[j][1]=yuandian.cy() j=j+1 if j==2: #只发现两个亮点 LED(3).off() #关闭蓝灯 #计算两个亮点到初始红点的距离，谁近则谁就是追踪点，远的就是目标点 r[0]=math.sqrt((ss[0][0]-red_x)(ss[0][0]-red_x) + (ss[0][1]-red_y)(ss[0][1]-red_y)) r[1]=math.sqrt((ss[1][0]-red_x)(ss[1][0]-red_x) + (ss[1][1]-red_y)(ss[1][1]-red_y)) #计算两个亮点之间的距离 rr =math.sqrt((ss[0][0]-ss[1][0])(ss[0][0]-ss[1][0]) + (ss[0][1]-ss[1][1])(ss[0][1]-ss[1][1])) if rr>20: #当两个亮点距离比较大的时候，动态的修正red_x，和red_y#即将距离近的点坐标幅值给初始红坐标。 if (r[0]<r[1]) and (r[0]<30) : red_x = ss[0][0] red_y = ss[0][1] if (r[1]<r[0]) and (r[1]<30) : red_x = ss[1][0] red_y = ss[1][1] if rr<6: #如果两点之间的距离小于6个像素，则认为已经跟踪上了 LED(2).on() #点亮绿灯 else: LED(2).off() #熄灭绿灯 now_x=pan_servo.pulse_width() now_y=tilt_servo.pulse_width() dx = ss[0][0]-ss[1][0] dy = ss[0][1]-ss[1][1] if r[0]>r[1]: # 再次根据远近判断红绿点，并确定距离方向。因为两点初始距离若小于等于20，则程序没有对预设的红点坐标进行动态校正。 dx=-dx dy=-dy if p_in1.value()==0: #暂停mov_pwm while(True): if p_in2.value()==0: #恢复mov_pwm break if dx<-5: pan_servo.pulse_width(now_x-10) if dy>5: tilt_servo.pulse_width(now_y+10) if dy<-5: tilt_servo.pulse_width(now_y-10) if math.sqrt(dxdx + dy*dy)>30 : #根据距离不同设置休眠时间 time.sleep_ms(10) else: time.sleep_ms(50) print("1111111111111111111111111111111111")
请在这里粘贴代码