class RobotController:
def __init__(self):
# camera = CameraController()
# camera.start()
self.communication = CommunicationController()
self.communication.listenToRefree()
print("robot initialized")
def Stop(self):
self.communication.sendCommand(0, 0, 0, 1)
print("Stop playing")
def Play(self):
self.communication.sendCommand(15, 15, 15, 1)
print("Playing")
def getBall(self):
print("ball obtained")
def hasBall(self):
return False
def searchBall(self):
print("Searching for ball")
class CameraController:
global communication
def __init__(self):
self.communication = CommunicationController()
self.count = 0
print("Camera Started")
def evaluate(self, ballContour, frame):
areaArray = []
if len(ballContour) > 0 :
for i, c in enumerate(ballContour):
area = cv2.contourArea(c)
areaArray.append(area)
bc_b = (sorted(zip(areaArray, ballContour), key=lambda x: x[0], reverse=True))[0][1]
centre_ball = []
moments_b = cv2.moments(bc_b)
centre_ball.append(int(moments_b['m10'] / moments_b['m00']))
centre_ball.append(int(moments_b['m01'] / moments_b['m00']))
cv2.circle(frame, (centre_ball[0], centre_ball[1]), 3, (0, 0, 0), -1)
x, y, w, h = cv2.boundingRect(bc_b)
cv2.drawContours(frame, bc_b, -1, (0, 165, 255), 2)
# Kerttu: calculating the radius for the ball by taking two extreme points from the contour: left and right
ball_radius_calculated = int(math.sqrt(math.pow(
tuple(bc_b[bc_b[:, :, 0].argmin()][0])[0] - tuple(bc_b[bc_b[:, :, 0].argmax()][0])[0],
2) + math.pow(
tuple(bc_b[bc_b[:, :, 0].argmin()][0])[1] - tuple(bc_b[bc_b[:, :, 0].argmax()][0])[1],
2)) / 2)
ball_radius = ball_radius_calculated
# print(ball_radius)
if (centre_ball[0] > 330) and (ball_radius < 18):
self.communication.sendCommand(15,15,15,1)
if (centre_ball[0] > 330) and (ball_radius >= 18):
self.communication.sendCommand(15, 15, 15, 1)
# turn left
if (centre_ball[0] < 230) and (ball_radius < 18):
self.communication.sendCommand(-15, -15, -15, 1)
if (centre_ball[0] < 230) and (ball_radius >= 18):
self.communication.sendCommand(-15, -15, -15, 1)
# go forward
if (centre_ball[0] > 230) and (centre_ball[0] < 330) and ball_radius < 18:
self.communication.sendCommand(-30, 0, 30, 1)
if (centre_ball[0] > 230) and (centre_ball[0] < 330) and ball_radius > 18:
self.communication.sendCommand(-20, 0, 20, 1)
def start(self):
cap = cv2.VideoCapture(VIDEO_DEVICE)
while(cap.isOpened()):
self.communication.listenToRefree()
ret, frame = cap.read()
if ret:
frame = cv2.flip(frame,180)
frame = imutils.resize(frame, width=600)
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
ballBoundaries = [ GREEN_BOUNDARIES ]
# loop over the boundaries
for (lower, upper) in ballBoundaries:
mask = cv2.inRange(hsv, lower, upper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cv2.imshow("mask", mask)
cv2.imshow("images", frame)
ballContour, hierarchy = cv2.findContours(mask.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)[-2:]
#Ball not found yet
if len(ballContour) == 0:
self.communication.sendCommand(10, 10, 10, 1)
self.evaluate(ballContour, frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
else:
break
cap.release()
cv2.destroyAllWindows()
def open(self):
cap = cv2.VideoCapture(VIDEO_DEVICE)
while(cap.isOpened()):
ret, frame = cap.read()
if ret:
# frame = cv2.flip(frame,180)
frame = imutils.resize(frame, width=600)
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
ballBoundaries = [ GREEN_BOUNDARIES ]
# loop over the boundaries
for (lower, upper) in ballBoundaries:
mask = cv2.inRange(hsv, lower, upper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cv2.imshow("mask", mask)
cv2.imshow("images", frame)
ballContour, hierarchy = cv2.findContours(mask.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)[-2:]
#Ball not found yet
if len(ballContour) == 0:
self.communication.sendCommand(10, 10, 10, 1)
self.evaluate(ballContour, frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
else:
break
cap.release()
cv2.destroyAllWindows()