import?cv2
import?cv2.cv?as?cv
import?cv
import?picamera
import?picamera.array
import?math
import?numpy?as?np
import?zmq
import?time
import?serial?time?sys
from?PIL?import?Image


#?For?OpenCV2?image?display
IMAGE_WINDOW_NAME?=?‘YelloBarTracker‘
CONTROL_WINDOW_NAME?=?‘Control‘
MASK_WINDOW_NAME?=?‘Mask‘

#?For?socket?communication
port?=?‘5556‘
context?=?zmq.Context（）
socket?=?context.socket（zmq.PUB）

#?Setting?the?initial?mask?threshold?
#?根據環境調試數據以便準確的捕捉小球
iLowH?=?5
iHighH?=?22

iLowS?=?219
iHighS?=?255

iLowV?=?149
iHighV?=?255

#?是否抓到球
getball?=?0

#?connect?arduino
#?將arduino接到樹莓派上輸入ls?/dev查看我的是ttyUSB0
def?connect_arduino（）:
????arduino?=?serial.Serial（‘/dev/ttyUSB0‘9600timeout=1）
????arduino.close（）
????arduino.open（）
????return?arduino

#?Require?by?cv2.createTrackbar.?we?have?nothing?to?do?with?nothing?method
def?nothing（var）:
????pass

def?connect（）:
????print（‘Getting?data?from?camera...‘）
????socket.bind（‘tcp://*:%s‘?%?port）

#?Create?trackbars?for?easier?adjustment?of?the?HSV?threshold??方便手動調節區間
def?make_hsv_adjustment（）:
????cv2.namedWindow（CONTROL_WINDOW_NAME）
????cv2.createTrackbar（‘LowH‘?CONTROL_WINDOW_NAME?iLowH?255?nothing）;?#Hue?（0?-?179）
????cv2.createTrackbar（‘HighH‘?CONTROL_WINDOW_NAME?iHighH?255?nothing）;

????cv2.createTrackbar（‘LowS‘?CONTROL_WINDOW_NAME?iLowS?255?nothing）;?#Saturation?（0?-?255）
????cv2.createTrackbar（‘HighS‘?CONTROL_WINDOW_NAME?iHighS?255?nothing）;

????cv2.createTrackbar（‘LowV‘?CONTROL_WINDOW_NAME?iLowV?255?nothing）;?#Value?（0?-?255）
????cv2.createTrackbar（‘HighV‘?CONTROL_WINDOW_NAME?iHighV?255?nothing）;

def?track（image）:

????‘‘‘Accepts?BGR?image?as?Numpy?array
???????Returns:?（xy）?coordinates?of?centroid?if?found
????????????????（-1-1）?if?no?centroid?was?found
????????????????None?if?user?hit?ESC
????‘‘‘

????#?Blur?the?image?to?reduce?noise
????blur?=?cv2.GaussianBlur（image?（55）0）

????#?Convert?BGR?to?HSV
????hsv?=?cv2.cvtColor（blur?cv2.COLOR_BGR2HSV）

????#?Get?the?treshold?from?the?trackbars
????iLowH?=?cv2.getTrackbarPos（‘LowH‘?CONTROL_WINDOW_NAME）
????iHighH?=?cv2.getTrackbarPos（‘HighH‘?CONTROL_WINDOW_NAME）
????iLowS?=?cv2.getTrackbarPos（‘LowS‘?CONTROL_WINDOW_NAME）
????iHighS?=?cv2.getTrackbarPos（‘HighS‘?CONTROL_WINDOW_NAME）
????iLowV?=?cv2.getTrackbarPos（‘LowV‘?CONTROL_WINDOW_NAME）
????iHighV?=?cv2.getTrackbarPos（‘HighV‘?CONTROL_WINDOW_NAME）

????#?Threshold?the?HSV?image?for?only?green?colors
????lower_yellow?=?np.array（[iLowHiLowSiLowV]）
????upper_yellow?=?np.array（[iHighHiHighSiHighV]）

????#?Threshold?the?HSV?image?to?get?only?yellow?colors
????mask?=?cv2.inRange（hsv?lower_yellow?upper_yellow）
????cv2.imshow（MASK_WINDOW_NAME?mask）

????#?Blur?the?mask
????bmask?=?cv2.GaussianBlur（mask?（55）0）

????#?Take?the?moments?to?get?the?centroid
????moments?=?cv2.moments（bmask）
????m00?=?moments[‘m00‘]
????centroid_x?centroid_y?radius?=?None?None?N