import sensor, image, time, pyb, mathfrom pyb import UARTfrom pyb import millisfrom math import pi, isnanfrom pyb import Servoclass PID: _kp = _ki = _kd = _integrator = _imax = 0 _last_error = _last_derivative = _last_t = 0 _RC = 1/(2 * pi * 20) def __init__(self, p=0, i=0, d=0, imax=0): self._kp = float(p) self._ki = float(i) self._kd = float(d) self._imax = abs(imax) self._last_derivative = float('nan') def get_pid(self, error, scaler): tnow = millis() dt = tnow - self._last_t output = 0 if self._last_t == 0 or dt > 1000: dt = 0 self.reset_I() self._last_t = tnow delta_time = float(dt) / float(1000) output += error * self._kp if abs(self._kd) > 0 and dt > 0: if isnan(self._last_derivative): derivative = 0 self._last_derivative = 0 else: derivative = (error - self._last_error) / delta_time derivative = self._last_derivative + \ ((delta_time / (self._RC + delta_time)) * \ (derivative - self._last_derivative)) self._last_error = error self._last_derivative = derivative output += self._kd * derivative output *= scaler if abs(self._ki) > 0 and dt > 0: self._integrator += (error * self._ki) * scaler * delta_time if self._integrator < -self._imax: self._integrator = -self._imax elif self._integrator > self._imax: self._integrator = self._imax output += self._integrator return output def reset_I(self): self._integrator = 0 self._last_derivative = float('nan')pan_servo=Servo(1)#水平舵机左右转动tilt_servo=Servo(2)#上方舵机俯仰转动pan_servo.calibration(500,2500,500)tilt_servo.calibration(500,2500,500)pan_servo.angle(0,1000)#水平舵机复位角度初始化tilt_servo.angle(0,1000)#上方舵机复位角度初始化red_threshold = (13, 49, 18, 61, 6, 47)pan_pid = PID(p=0.07, i=0, imax=90) #脱机运行或者禁用图像传输,使用这个PIDtilt_pid = PID(p=0.05, i=0, imax=90) #脱机运行或者禁用图像传输,使用这个PID#pan_pid = PID(p=0.1, i=0, imax=90)#在线调试使用这个PID#tilt_pid = PID(p=0.1, i=0, imax=90)#在线调试使用这个PIDuart = UART(3, 115200)uart.init(115200, bits=8, parity=None, stop=1)sensor.reset()sensor.set_pixformat(sensor.RGB565)sensor.set_framesize(sensor.QVGA)sensor.skip_frames(time=1000)sensor.set_auto_gain(True) # 颜色跟踪必须关闭自动增益sensor.set_auto_whitebal(True) # 颜色跟踪必须关闭白平衡# sensor.set_vflip(True)# sensor.set_hmirror(True)# LED1=pyb.LED(1)#pyb是模块,led是其中一个类# LED2=pyb.LED(2)# LED3=pyb.LED(3)# LED1.on()# LED2.on()# LED3.on()clock = time.clock()threshold_blackblob=[(4, 29, -24, 6, -5, 16)]#(16, 29, -16, 1, 0, 14)min_area=1600max_area=41500max_density=0.2max_solidity=0.4max_convexity=0.6def line_intersection(line1, line2): """ 计算两条直线的交点坐标 """ (x1, y1, x2, y2) = line1 (x3, y3,x4, y4) = line2 # 计算分母(判断是否平行) den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4) if den == 0: # 分母为0,两直线平行或重合,无唯一交点 return (abs(x2-x1)//2,abs(y2-y1)//2) # 计算分子 t_num = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4) s_num = (x1 - x3) * (y1 - y2) - (y1 - y3) * (x1 - x2) t = t_num / den s = s_num / den # 计算交点坐标 x = x1 + t * (x2 - x1) y = y1 + t * (y2 - y1) return (x, y)while(True): valid_blobs = [] clock.tick() ##先串口接收 if uart.any(): pt = uart.readchar() pt = chr(pt) # print(pt) # pt=uart.readline().decode() # print(pt) ##拍照 img = sensor.snapshot() blobs = img.find_blobs(threshold_blackblob,pixels_threshold=1, area_threshold=1,x_stride=1,y_stride=1,margin=15) if blobs: valid_blobs = [] for blob in blobs: if blob.density()<max_density: if blob.area()>min_area and blob.area()<max_area: if blob.w()>blob.h(): #print(blob.density(),blob.solidity(),blob.convexity())#像素数除以其边界框区域,使用最小区域旋转矩形与边界矩形来测量密度,对象的凸度 ##0.152698 0.167296 0.423312 # valid_blobs.append(blob) if blob.solidity()<max_solidity: if blob.convexity()<max_convexity: valid_blobs.append(blob) if valid_blobs: min_density_blob = min(valid_blobs, key=lambda b: b.density()) img.draw_line(min_density_blob.major_axis_line()) img.draw_line(min_density_blob.minor_axis_line()) intersection=line_intersection(min_density_blob.major_axis_line(), min_density_blob.minor_axis_line()) lx1=abs(min_density_blob.major_axis_line()[0]-min_density_blob.major_axis_line()[2]) ly1=abs(min_density_blob.major_axis_line()[1]-min_density_blob.major_axis_line()[3]) l1=lx1**2+ly1**2 l1=l1**(1/2) lx2=abs(min_density_blob.minor_axis_line()[0]-min_density_blob.minor_axis_line()[2]) ly2=abs(min_density_blob.minor_axis_line()[1]-min_density_blob.minor_axis_line()[3]) l2=lx2**2+ly2**2 l2=l2**(1/2) # print(l2/l1) if l2/l1>0.7: img.draw_rectangle(min_density_blob.rect(), color=(255,0,0)) # 红色矩形框 img.draw_cross(min_density_blob.min_corners()[0][0], min_density_blob.min_corners()[0][1]) img.draw_cross(min_density_blob.min_corners()[1][0], min_density_blob.min_corners()[1][1]) img.draw_cross(min_density_blob.min_corners()[2][0], min_density_blob.min_corners()[2][1]) img.draw_cross(min_density_blob.min_corners()[3][0], min_density_blob.min_corners()[3][1]) img.draw_cross(int(intersection[0]),int(intersection[1]), color=(255,0,0)) print(intersection)#靶心坐标 pan_error = intersection[0]-img.width()/2 tilt_error = intersection[1]-img.height()/2 pan_output=pan_pid.get_pid(pan_error,1)/2 tilt_output=tilt_pid.get_pid(tilt_error,1) print("pan_output",pan_output) print("tilt_output",tilt_output) pan_servo.angle(pan_servo.angle()+pan_output)#水平舵机左右转动 tilt_servo.angle(tilt_servo.angle()-tilt_output)#上方舵机俯仰转动 FH = bytearray([0x2C,0x12,int(intersection[0]),int(intersection[1]),0x00,0x00,0x5B])
uart.write(FH)