import?sensor?image?time

from?pid?import?PID
from?pyb?import?ServoUARTPin
import?pyb

uart?=?UART（3?115200）?#?TX:P4?RX:P5?（板子線反接）
pan_servo=Servo（1）?#?P7
distance_servo=Servo（2）?#?P8
key_servo?=?Servo（3）?#?P9?-40:開?40：關
shot_flagpin?=?Pin（‘P1‘?Pin.OUT_PP）?#?設置shot_flagpin為輸出引腳
shot_flagpin.low（）?#?拉高發射控制電平?高電平：發射?低電平：不發射
mode_Highpin?=?Pin（‘P2‘?Pin.IN?Pin.PULL_UP）#?設置flag_Highpin為輸入引腳，并開啟上拉電阻
mode_Lowpin?=?Pin（‘P3‘?Pin.IN?Pin.PULL_UP）#?設置flag_Lowpin為輸入引腳，并開啟上拉電阻


distance_dict?=?{‘15‘:212‘16‘:214‘17‘:229‘18‘:232‘19‘:234‘20‘:242‘21‘:248‘22‘:252‘23‘:260‘24‘:272‘25‘:273‘26‘:270‘27‘:264‘28‘:274‘29‘:296‘30‘:283‘31‘:300}

mode_flag?=?0?#?0:等待模式??1:手動模式?2:自動模式?3：擺頭模式
uart_flag?=?False?#?True:串口接收完成?False:?串口等待接收
shot_flag?=?False?#?True:發射炮彈?False:?不發射炮彈
distance_finish?=?False?#?True:?調整完成?False:未調整完成
pan_finish?=?False?#?True:?調整完成?False:未調整完成
distance_rejustvalue?=?0?#?調試上傳調整距離，解決閾值變化問題


#?數據包起始標志位
START?=?b‘（‘
STOP?=?b‘）‘
mid_Xangel?=?30?#X?舵機給定初始轉向?30
mid_Yangel?=?15#Y?舵機給定初始轉向?15
X_range?=?list（range（（mid_Xangel-30）（mid_Xangel+30）））?#?模式三的旋轉范圍
X_i?=?0?#?旋轉計數
Aim_threshold?=?（27?58?46?74?3?59）#?（34?61?25?66?-11?47）
red_threshold??=?（13?49?18?61?6?47）
pixels_threshold??=?60
K=?4700?#the?value?should?be?measured

#pan_pid?=?PID（p=0.07?i=0?imax=90）?#脫機運行或者禁用圖像傳輸，使用這個PID
#distance_pid?=?PID（p=0.05?i=0?imax=90）?#脫機運行或者禁用圖像傳輸，使用這個PID
pan_pid?=?PID（p=0.1?i=0?imax=90）#在線調試使用這個PID
distance_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.QVGA）?#?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?mode_read（）:
????High?=?mode_Highpin.value（）
????Low?=?mode_Lowpin.value（）
????modevalue?=?High?*?2?+?Low?*?1
????return?modevalue

#?找最大標志點
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

#?距離角度轉換?distabce?=?angle?*?-2.1786?+?400.96
def?distance_turn2angel（distance）:
????angel?=?int（（distance?+?distance_rejustvalue?-?139.316）/5）?#?乘以一個系數
????return?angel

#?發射開關
def?is_shot（is_fire）:
????#?-40?guang?40?kai
????if?is_fire:
????????key_servo.angle（10）
????else:
????????key_servo.angle（55）

#?轉角限幅
def?turn_max（）:
????pass

#?初始化
pan_servo.angle（mid_Xangel）#?擺正水平舵機
distance_servo.angle（mid_Yangel）?#?擺正豎直舵機
is_shot（False）



while（True）:
????clock.tick（）?#?Track?elapsed?milliseconds?between?snapshots（）.
????mode_flag?=?mode_read（）?#?獲取運行模式
????if?mode_flag?==?0:
????????#?mode_flag?=?0
????????pan_servo.angle（mid_Xangel）#?擺正水平舵機
????????distance_servo.angle（mid_Yangel）?#?擺正豎直舵機
????????is_shot（False）
????????uart_flag?=?False
????????shot_flag?=?False?#?