SCRIPT PYTHON

mail@pastecode.io avatar
unknown
plain_text
3 years ago
2.7 kB
37
Indexable
Never
from collections import deque
import numpy as np
import argparse
import imutils
import cv2
import sys
import serial
import time
import math

#arduino = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=.1)
arduino = serial.Serial(port='COM11', baudrate=115200, timeout=.1)

def write_read(x):
    arduino.write(bytes(x, 'utf-8'))
    time.sleep(0.05)
    data = arduino.readline().decode('utf-8').rstrip()
    return data
PY3 = sys.version_info[0] == 3
if PY3:
	xrange = range

ap = argparse.ArgumentParser()
ap.add_argument("-v", "--video",
	help="path to the (optional) video file")
ap.add_argument("-b", "--buffer", type=int, default=64,
	help="max buffer size")
args = vars(ap.parse_args())

ballLower = (4, 130, 70)
ballUpper = (15, 243, 245)
pts = deque(maxlen=args["buffer"])

if not args.get("video", False):
	camera = cv2.VideoCapture(0)

else:
	camera = cv2.VideoCapture(args["video"])

while True:
	(grabbed, frame) = camera.read()

	if args.get("video") and not grabbed:
		break

	frame = imutils.resize(frame, width=600)
	#blurred = cv2.GaussianBlur(frame, (11, 11), 0)
	hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

	mask = cv2.inRange(hsv, ballLower, ballUpper)
	mask = cv2.erode(mask, None, iterations=2)
	mask = cv2.dilate(mask, None, iterations=2)

	cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
		cv2.CHAIN_APPROX_SIMPLE)[-2]
	center = None

	if len(cnts) > 0:
		c = max(cnts, key=cv2.contourArea)
		((x, y), radius) = cv2.minEnclosingCircle(c)
		M = cv2.moments(c)
		center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
		#pusat = list(center)
		u = list(center)
		posX = u[0]//10
		xball = posX - 32
		posY = u[1]//10
		yball = 48 - posY
		r = math.sqrt((posX*posX)+(posY*posY))
		tetha = math.atan2(yball,xball)
		pi = 22/7
		degree = round(tetha*(180/pi))
		#ser.write(str(degree).encode() + b'\n\r')
		#print(str(posX))
		tetha = round(math.degrees(math.asin(xball/r)),2)
		

		
		if radius > 10:
			cv2.circle(frame, (int(x), int(y)), int(radius),
				(0, 255, 255), 2)
			cv2.circle(frame, center, 5, (0, 0, 255), -1)
			posX = int(x)
			posY = int(y)
			jarakX = int(x); #rumus kalibrasi X, dalam cm
			jarakY = int(y); #rumus kalibrasi Y, dalam cm
			#sudut = int(math.degrees(math.atan(jarakX/jarakY)))
			#print(int(tetha))
			degree = str(degree)
			value = write_read(degree)
			#arduino.write(degree)
			arduino.write(value)
			print(degree)
			#time.sleep(0.1)

	pts.appendleft(center)

	cv2.imshow("Frame", frame)
	key = cv2.waitKey(1) & 0xFF
	if key == ord("q"):
		break
 
camera.release()
cv2.destroyAllWindows()