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 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

#，代码已经保存到openmv了，通过ide然后usb插电脑上供电就能跑，但是用5v供电就不能跑，usb用个电源插头插插排上也不能跑，代码已经保存了，插一个内存卡也不行

import sensor, image, time,ustruct
from pyb import UART,LED
from pyb import UART,LED

#摄像头
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)#转为灰度图
sensor.set_framesize(sensor.QVGA)#分辨率
sensor.skip_frames(time = 2000)#初始给2秒的停顿让相机稳定
sensor.set_auto_gain(False)#灰度图巡迹的时候建议关闭自动增益调节功能，可能会影响要二值图的完整性。
sensor.set_auto_whitebal(False)#同样也不建议开启，白平衡
sensor.set_vflip(True)# 垂直方向翻转
uart = UART(3,115200)#通信的波特率设置，与主板需要设置成一样才能进行通信
uart.init(115200, bits=8, parity=None, stop=1) # init with given parameters

def sending_data(cx):
global uart;
#frame=[0x2C,18,cx%0xff,int(cx/0xff),cy%0xff,int(cy/0xff),0x5B];
#data = bytearray(frame)
data = ustruct.pack("<bbhhhhb", #格式为俩个字符俩个短整型(2字节)
0x2C, #帧头1
0x12, #帧头2
int(cx), # up sample by 4 #数据1
0x5B)
uart.write(data); #必须要传入一个字节数组

#补光
LED(1).on()
LED(2).on()
LED(3).on()

clock = time.clock()
#图像分区(0上1中左2中右3下面那一大块)
roi1 = [(73,10,154,49),
(30,93,69,72),
(200,82,108,69),
(0,79,320,162)]
thresholds1 = (35,0)
THRESHOLD = (0, 95)

while(True):
#霍夫变换小车官方例程改写
clock.tick()
img = sensor.snapshot().binary([THRESHOLD])
line = img.get_regression([(255,255)],roi=roi1[3])
if (line):
rho_err = abs(line.rho())-img.width()/2
if line.theta()>90:
theta_err = line.theta()-180
else:
theta_err = line.theta()
img.draw_line(line.line(), color = 127)
print(rho_err)
sending_data(rho_err)
if line.magnitude():
#绘制框图
left_flag=0
for rec in roi1:
img.draw_rectangle(rec, color=(255,0,0))
#根据框内有多少白色像素判断情况
if img.find_blobs([(255, 95)],roi=roi1[1],area_threshold=3000):
left_flag=1
print(left_flag)