基于OpenMv的火焰追踪报警系统

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
import sensor, image, time, utime, pyb
from pid import PID
from pyb import Servo
from pyb import Pin

wave_echo_pin = Pin('P0', Pin.IN, Pin.PULL_NONE)
wave_trig_pin = Pin('P1', Pin.OUT_PP, Pin.PULL_DOWN)

wave_distance = 0
tim_counter = 0
flag_wave = 0
pan_servo=Servo(1)
tilt_servo=Servo(2)
pan_servo.calibration(500,2500,500)
tilt_servo.calibration(500,2500,500)

red_threshold  = (25, 47, 31, 75, 17, 55)

#pan_pid = PID(p=0.07, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
#tilt_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)#在线调试使用这个PID

sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.

def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob

#超声波启动
def wave_start():
    #print("6")
    wave_trig_pin.value(0)
    utime.sleep_us(2)
    wave_trig_pin.value(1)
    utime.sleep_us(20)
    wave_trig_pin.value(0)

#超声波距离计算
def wave_distance_calculation():
    #全局变量声明
    global tim_counter
    #频率f为0.2MHZ 高电平时间t=计数值*1/f
    wave_distance = tim_counter*5*0.017
    #输出最终的测量距离（单位cm）
    print('wave_distance',wave_distance)

#超声波数据处理
def wave_distance_process():
    global flag_wave
    #extint.disable()
    if(flag_wave == 0):
        #print("4")
        wave_start()
        return
    if(flag_wave == 2):
        #print("5")
        wave_distance_calculation()
        return True


#配置定时器
tim =pyb.Timer(2, prescaler=720, period=65535)  #相当于freq=0.2M

#外部中断配置
def callback(line):
    global flag_wave,tim_counter
    #上升沿触发处理
    if(wave_echo_pin.value()):
        tim.init(prescaler=720, period=65535)
        flag_wave = 1
    #下降沿
    else:
        tim.deinit()
        tim_counter = tim.counter()
        tim.counter(0)
        extint.disable()
        flag_wave = 2


##云台运动
def servo_run():
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.
    ang = 0
    blobs = img.find_blobs([red_threshold])
    if blobs:
        max_blob = find_max(blobs)
        pan_error = img.width()/2-max_blob.cx()
        tilt_error = img.height()/2-max_blob.cy()

        #print("pan_error: ", pan_error)

        img.draw_rectangle(max_blob.rect()) # rect
        img.draw_cross(max_blob.cx(), max_blob.cy()) # cx, cy

        pan_output=pan_pid.get_pid(pan_error,1)/2
        tilt_output=tilt_pid.get_pid(tilt_error,1)
        #print("pan_output",pan_output)
        pan_servo.angle(pan_servo.angle()+pan_output)
        tilt_servo.angle(tilt_servo.angle()-tilt_output)
    else:
        #pan_servo.angle(90)
        if ang < 49:
            ang += 2
            print("pan_servo.angle()",pan_servo.angle())
            pan_servo.angle(pan_servo.angle()+ang)
            #tilt_servo.angle(pan_servo.angle()-ang)
        else:
            pass
            



#外设
def other_run():
    laser_p6_out.value(1)
    if(fire_p2_in.value()==0 and smoke_p4_in.value()==0):# get value, 0 or 1 火焰
        print("find fire!!!")
        laser_p6_out.value(0)
        beer_p3_out.high()  #beer
        time.sleep_ms(500)
        beer_p3_out.low()  #beer
        time.sleep_ms(500)

fire_p2_in = Pin('P2', Pin.IN, Pin.PULL_UP)##火焰
beer_p3_out = Pin('P3', Pin.OUT_PP)
beer_p3_out.value(0)
smoke_p4_in = Pin('P4', Pin.IN, Pin.PULL_UP)##烟雾
laser_p6_out = Pin('P6', Pin.OUT_PP)#激光模块，灭火器
laser_p6_out.value(1)
#中断配置
extint = pyb.ExtInt(wave_echo_pin, pyb.ExtInt.IRQ_RISING_FALLING, pyb.Pin.PULL_DOWN, callback)
ok = 0
while(True):
    ##超声波
    extint.enable()
    while(True):
        ok = wave_distance_process()#执行计算距离
        #print("start!")
        if ok:
            ok = 0
            break
        time.sleep_ms(5)

    wave_echo_pin = Pin('P0', Pin.IN, Pin.PULL_NONE)
    wave_trig_pin = Pin('P1', Pin.OUT_PP, Pin.PULL_DOWN)
    wave_distance = 0
    tim_counter = 0
    flag_wave = 0
    extint.disable()

    other_run()#火焰and烟雾检测
    servo_run()#云台控制