def __init__(self):
self.description = "Strategy with no gameplay mechanics."
# DEBUG
self.debugLines = []
self.debugPoints = []
self.debugString = ""
# Striker
self.striker = StrategyStriker()
self.opponentStriker = StrategyStriker()
# Striker Limits
self.maxSpeed = MAX_SPEED
self.acceleration = MAX_ACCELERATION
self.deceleration = MAX_DECELERATION
self.gain = KP_GAIN
# Puck
self.puck = StrategyPuck()
self.puckHistory = []
self.puckHistory.append(self.puck)
self.lastMove = 0
# Trajectory info
self.goalLineIntersection = 0
self.willBounce = False
# Parameters
self.historySize = 50
self.noOfBounces = 1
self.minSpeedLimit = 100
self.highAngleTolerance = 50
self.mediumAngleTolerance = 15
self.lowAngleTolerance = 8
self.positionTolerance = 100
self.capturesWithBadLowAngle = 0
self.capturesWithBadMediumAngle = 0
# States
self.stepTime = 0
self.gameTime = 0
self.timeSinceLastCameraInput = 0
self.sameCameraInputsInRow = 0
self.previousErrorSide = 0
self.firstUsefull = 1
self.predictedPosition = Vector2(0, 0)
# Filter
self.angleFilter = Filter(15, 2, 1, isVector=False)
# Init
for i in range(self.historySize - 1):
self.puckHistory.append(StrategyPuck())
def __init__(self, game, fps): self.game = game self.filter = Filter(15, 2.2, 1.2) self.delay = 0.02 # in seconds - will not be precise self.frameRate = fps self.stepsSinceLastCapture = 0 self.newData = False self.puckPosition = Vector2(0, 0) self.positionHistory = [] self.xGrid = [] self.yGrid = [] self.createGrid(4) self.historySize = max(round(self.delay/(1/self.frameRate)),1)
def __init__(self, settings=None): # 320, 192
if settings is None:
settings = Settings('AirHockey_settings.obj')
self.settings = settings.camera
else:
self.settings = settings
self.piVideo = None
self.camera = None
self.detectionStopped = True
self.analyzingStopped = True
# self.findingFieldStopped = True
self.lockingAwbStopped = True
self.counter = FPSCounter(movingAverage=120).start()
self.detectingCounter = FPSCounter(movingAverage=120)
self.frame = None
self.mask = None
self.filteredMask = None
self.cursorPosition = None
self.frameCount = 0
# self._determineColorIntervals()
self.p2uTranformMatrix = None
self.u2pTranformMatrix = None
self.prevFieldCorners = None
self._createTransformMatrices(self.settings["fieldCorners"].copy())
self.newPosition = False
self.pixelPuckPosition = Vector2(int(0), int(0))
self.unitPuckPosition = Vector2(0, 0)
self.unitFilteredPuckPosition = Vector2(0, 0)
self.filter = Filter(*self.settings["filterConstants"])
self.callback = self._nothing
class Camera():
def __init__(self, settings=None): # 320, 192
if settings is None:
settings = Settings('AirHockey_settings.obj')
self.settings = settings.camera
else:
self.settings = settings
self.piVideo = None
self.camera = None
self.detectionStopped = True
self.analyzingStopped = True
# self.findingFieldStopped = True
self.lockingAwbStopped = True
self.counter = FPSCounter(movingAverage=120).start()
self.detectingCounter = FPSCounter(movingAverage=120)
self.frame = None
self.mask = None
self.filteredMask = None
self.cursorPosition = None
self.frameCount = 0
# self._determineColorIntervals()
self.p2uTranformMatrix = None
self.u2pTranformMatrix = None
self.prevFieldCorners = None
self._createTransformMatrices(self.settings["fieldCorners"].copy())
self.newPosition = False
self.pixelPuckPosition = Vector2(int(0), int(0))
self.unitPuckPosition = Vector2(0, 0)
self.unitFilteredPuckPosition = Vector2(0, 0)
self.filter = Filter(*self.settings["filterConstants"])
self.callback = self._nothing
def lockCameraAwb(self):
print("Calibrating...")
# Get auto-set values
rg, bg = self.camera.awb_gains
prevGains = (rg, bg)
print("Warming up...")
time.sleep(1.0)
print("Done")
frameCount = 0
while frameCount < 300:
if self.piVideo.newFrame:
self.frame = self.piVideo.read()
frameCount += 1
greenPart = np.repeat(self.frame[:, :, 1],
3).reshape(self.frame.shape)
diffToWhite = (self.frame.astype("int16") -
greenPart.astype("int16"))
if frameCount == 1:
# Get reference pixels
vectorized = diffToWhite.reshape((-1, 3)).astype("float32")
K = 8
attempts = 10
criteria = (cv2.TERM_CRITERIA_EPS +
cv2.TERM_CRITERIA_MAX_ITER, 10, 1.0)
ret, label, center = cv2.kmeans(vectorized, K, None,
criteria, attempts,
cv2.KMEANS_PP_CENTERS)
labels = label.flatten()
mostFrequentLabel = np.argmax(np.bincount(labels))
referenceIndexes = [
random.choice(
np.argwhere(labels == mostFrequentLabel))[0]
for x in range(10)
]
mask = (labels == mostFrequentLabel).reshape(
self.frame.shape[0],
self.frame.shape[1]).astype("uint8")
# Get reference pixel for current iteration
referencePixel = diffToWhite.reshape(
(-1, 3))[random.choice(referenceIndexes)]
if abs(referencePixel[0]) < 5 and abs(referencePixel[2]) < 5:
break # If good enough -> break
# Set white balance iterably
rg -= referencePixel[2] / 500
bg -= referencePixel[0] / 500
self.settings["whiteBalance"] = [
max(min(rg, 8), 0), max(min(bg, 8), 0)
]
self.setWhiteBalance()
self.frame = cv2.bitwise_and(self.frame, self.frame, mask=mask)
# cv2.imshow("Calibrating", self.frame)
cv2.waitKey(1)
time.sleep(0.2)
rg, bg = self.camera.awb_gains
if rg < 0.1 or bg < 0.1:
results = "Failed to find sufficient gains.\nTry again."
self.camera.awb_gains = prevGains
else:
results = "Set white balance:\nRed gain: {}\nBlue gain: {}".format(
round(float(rg), 1), round(float(bg), 1))
self.lockingAwbStopped = True
self.callback(results)
# print(results)
return
# cv2.destroyWindow("Calibrating")
# cv2.waitKey(1)
# def findField(self):
# # TODO
# self.settings["fieldCorners"] = self.settings["fieldCorners"]
# self._calibrateField()
# self.findingFieldStopped = True
def analyzeColor(self):
started = time.time()
secondLeft = 3
print("Analyzing most domiant color...")
print("Saving in: " + str(secondLeft))
secondLeft -= 1
while True:
if self.piVideo.newFrame:
self.frame = self.piVideo.read()
frame = self.frame[
round(self.settings["resolution"][1] *
0.2):round(self.settings["resolution"][1] * 0.8),
round(self.settings["resolution"][0] *
0.2):round(self.settings["resolution"][0] * 0.8)]
frame = cv2.GaussianBlur(frame, (11, 11), 0)
# cv2.imshow("Analyzing", frame)
if time.time() - started > 1:
print(secondLeft)
secondLeft -= 1
started = time.time()
vectorized = frame.reshape((-1, 3)).astype("float32")
K = 3
attempts = 10
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,
10, 1.0)
ret, label, center = cv2.kmeans(vectorized, K, None, criteria,
attempts,
cv2.KMEANS_PP_CENTERS)
labels = label.flatten()
mostFrequentLabel = np.argmax(np.bincount(labels))
self.settings["colorToDetect"] = detectedColor = cv2.cvtColor(
np.uint8([[center[mostFrequentLabel]]]),
cv2.COLOR_BGR2HSV)[0, 0, :]
self._determineColorIntervals()
# Create a blank 300x300 black image
foundColorFrame = np.zeros((100, 100, 3), np.uint8)
# Fill image with red color(set each pixel to red)
foundColorFrame[:] = np.uint8([[center[mostFrequentLabel]]
])[0, 0, :]
# cv2.imshow("FoundColor", foundColorFrame)
cv2.waitKey(1)
if secondLeft == 0:
print("Saving found color...")
self._determineColorIntervals()
break
# cv2.destroyWindow("Analyzing")
# cv2.destroyWindow("FoundColor")
# cv2.waitKey(1)
results = "Found color: {}\n Set limits: {} {}".format(
detectedColor, self.settings["lowerLimits"],
self.settings["upperLimits"])
self.callback(results)
# print(results)
self.analyzingStopped = True
def detectPuck(self):
if not MAX_PERFORMANCE:
cv2.namedWindow('Frame')
if HSV_TRACKBARS:
cv2.setMouseCallback('Frame', self._mouseHSV)
cv2.namedWindow("Trackbars")
cv2.createTrackbar("Hl", "Trackbars", 0, 179, self._nothing)
cv2.createTrackbar("Hh", "Trackbars", 0, 179, self._nothing)
cv2.setTrackbarPos("Hl", "Trackbars",
self.settings["lowerLimits"][0])
cv2.setTrackbarPos("Hh", "Trackbars",
self.settings["upperLimits"][0])
if WHITEBALANCE_TRACKBARS:
cv2.namedWindow("White balance")
cv2.createTrackbar("Red", "White balance", 0, 80,
self._nothing)
cv2.createTrackbar("Blue", "White balance", 0, 80,
self._nothing)
print("Detecting...")
while True:
if self.piVideo.newFrame:
self.frame = self.piVideo.read()
self.frameCount += 1
self.counter.tick()
self._createTransformMatrices(self.settings["fieldCorners"])
if ENABLE_BLURRING and not MAX_PERFORMANCE:
blurred = cv2.GaussianBlur(self.frame, (11, 11), 0)
frameHSV = cv2.cvtColor(blurred,
cv2.COLOR_BGR2HSV) # not worth
else:
frameHSV = cv2.cvtColor(self.frame, cv2.COLOR_BGR2HSV)
if HSV_TRACKBARS and not MAX_PERFORMANCE:
self.settings["lowerLimits"][0] = cv2.getTrackbarPos(
"Hl", "Trackbars")
self.settings["upperLimits"][0] = cv2.getTrackbarPos(
"Hh", "Trackbars")
# if DETECT_PUCK:
if self.settings["lowerLimits"][0] > self.settings[
"upperLimits"][0]:
lowerLimit1 = np.uint8(self.settings["lowerLimits"])
higherLimit1 = np.uint8([
179, self.settings["upperLimits"][1],
self.settings["upperLimits"][2]
])
lowerLimit2 = np.uint8([
0, self.settings["lowerLimits"][1],
self.settings["lowerLimits"][2]
])
higherLimit2 = np.uint8(self.settings["upperLimits"])
mask1 = cv2.inRange(frameHSV, lowerLimit1, higherLimit1)
mask2 = cv2.inRange(frameHSV, lowerLimit2, higherLimit2)
self.mask = cv2.bitwise_or(mask1, mask2)
else:
self.mask = cv2.inRange(frameHSV,
self.settings["lowerLimits"],
self.settings["upperLimits"])
# perform a series of dilations and erosions to remove any small blobs left in the self.mask
self.filteredMask = cv2.erode(self.mask, None, iterations=1)
self.filteredMask = cv2.dilate(self.filteredMask,
None,
iterations=1)
filtered = cv2.bitwise_and(self.frame,
self.frame,
mask=self.filteredMask)
#----------------------------- DETECTION -----------------------------
try:
cnts = cv2.findContours(self.filteredMask.copy(),
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
center = None
# only proceed if at least one contour was found
if len(cnts) > 0:
# find the largest contour in the mask, then use it to compute the minimum enclosing circle and centroid
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
# only proceed if the radius meets a minimum size
if radius > self.settings["limitPuckRadius"]:
self.detectingCounter.tick()
pixelPos = Vector2(int(x + 1), int(y + 3))
unitPos = self._pixelsToUnits(pixelPos)
if self._isPuckInField(unitPos):
self.pixelPuckPosition = pixelPos
self.unitPuckPosition = unitPos
self.unitFilteredPuckPosition = self.filter.filterData(
Vector2(self.unitPuckPosition[0],
self.unitPuckPosition[1]))
self.newPosition = True
except:
print("Error during puck detection.")
if not MAX_PERFORMANCE:
if SHOW_DETECTION:
self._drawField(self.settings["fieldCorners"])
self._drawPuck(self.pixelPuckPosition)
filteredPixelPos = self._unitsToPixels(
self.unitFilteredPuckPosition)
self._drawPuck(filteredPixelPos, color=(0, 255, 0))
# self._writeText(str(self.unitPuckPosition), (self.pixelPuckPosition[0] + 10, self.pixelPuckPosition[1] + 10), fontScale=0.5)
self._writeText(str(self.unitFilteredPuckPosition),
(filteredPixelPos[0] + 10,
filteredPixelPos[1] + 10),
fontScale=0.5)
# min enclosing circle
try:
cv2.circle(self.frame, self.pixelPuckPosition,
int(radius), (0, 255, 255), 2)
except:
pass
# Write info to frame
if SHOW_MOUSE_HSV:
try:
self._writeText("HSV: " + str(frameHSV[
self.cursorPosition.y, self.cursorPosition.x]))
except:
pass
if SHOW_FPS:
self._writeText(
"FPS: " +
str(round(self.counter.movingAverageFps)),
position=(10, 60),
fontScale=0.6)
if SHOW_CAPTURE_INFO:
self._writeText(
"Exposure: " +
str(self.piVideo.camera.exposure_speed),
position=(10, 80),
fontScale=0.6)
r, g = self.camera.awb_gains
self._writeText("AWB Gains: " + str(
(round(float(r), 1), round(float(g), 1))),
position=(10, 100),
fontScale=0.6)
self._writeText("a/d Gains: " + str(
(round(float(self.camera.analog_gain), 1),
round(float(self.camera.digital_gain), 1))),
position=(10, 120),
fontScale=0.6)
# Show image
if SHOW_MASK and DETECT_PUCK:
cv2.imshow("Mask", self.mask)
if SHOW_FILTERED_MASK and DETECT_PUCK:
cv2.imshow("Filtered mask", self.filteredMask)
cv2.imshow("Frame", self.frame)
if WHITEBALANCE_TRACKBARS:
rg = cv2.getTrackbarPos("Red", "White balance") / 10
bg = cv2.getTrackbarPos("Blue", "White balance") / 10
self.camera.awb_gains = (rg, bg)
key = cv2.waitKey(5)
if self.detectionStopped:
print("Detecting stopped.")
return
self.piVideo.stop()
cv2.destroyAllWindows()
def startCamera(self):
if self.piVideo is None:
self.piVideo = PiVideoStream(self.settings["resolution"],
self.settings["fps"],
self.settings["whiteBalance"])
self.camera = self.piVideo.camera
self.piVideo.start()
def stopCamera(self):
self.detectionStopped = True
self.analyzingStopped = True
self.findingFieldStopped = True
self.lockingAwbStopped = True
time.sleep(.1)
self.piVideo.stop()
self.piVideo = None
self.camera = None
def startDetecting(self):
if self.detectionStopped:
self.detectionStopped = False
self.detectingCounter.start()
Thread(target=self.detectPuck, args=()).start()
else:
print("Detecting thread already running.")
def stopDetecting(self):
self.detectingCounter.stop()
self.detectionStopped = True
def startAnalyzing(self, callback=None):
if callback is None:
self.callback = self._nothing
else:
self.callback = callback
if self.analyzingStopped:
self.analyzingStopped = False
Thread(target=self.analyzeColor, args=()).start()
else:
print("Analyzing thread already running.")
def stopAnalyzing(self):
self.analyzingStopped = True
# def startFindingField(self, callback = None):
# if callback is None:
# self.callback = self._nothing
# else:
# self.callback = callback
# if self.findingFieldStopped:
# self.findingFieldStopped = False
# Thread(target=self.findField, args=()).start()
# else:
# print("Finding field thread already running.")
# def stopFindingField(self):
# self.findingFieldStopped = True
def startLockingAwb(self, callback=None):
if callback is None:
self.callback = self._nothing
else:
self.callback = callback
if self.lockingAwbStopped:
self.lockingAwbStopped = False
Thread(target=self.lockCameraAwb, args=()).start()
else:
print("Locking AWB thread already running.")
def stopLockingAwb(self):
self.lockingAwbStopped = True
def getPuckPosition(self):
self.newPosition = False
return self.unitFilteredPuckPosition
def setWhiteBalance(self):
if self.camera is not None:
self.camera.awb_gains = (self.settings["whiteBalance"][0],
self.settings["whiteBalance"][1])
def _isPuckInField(self, pos):
if not (0 < pos.x < FIELD_WIDTH):
return False
if not (-FIELD_HEIGHT / 2 < pos.y < FIELD_HEIGHT / 2):
return False
return True
# def _calibrateField(self):
# # TODO: find the field
# self._createTransformMatrices(self.settings["fieldCorners"])
def _determineColorIntervals(self):
Hl = self.settings["colorToDetect"][0] - round(
self.settings["intervals"][0] / 2)
if Hl < 0: Hl += 179
Hh = self.settings["colorToDetect"][0] + round(
self.settings["intervals"][0] / 2)
if Hh > 179: Hh -= 179
self.settings["lowerLimits"] = np.uint8([
Hl,
max(
5, self.settings["colorToDetect"][1] -
round(self.settings["intervals"][1])),
max(
5, self.settings["colorToDetect"][2] -
round(self.settings["intervals"][2]))
])
self.settings["upperLimits"] = np.uint8([
Hh,
min(
255, self.settings["colorToDetect"][1] +
round(self.settings["intervals"][1] / 2)),
min(
255, self.settings["colorToDetect"][2] +
round(self.settings["intervals"][2] / 2))
])
def _nothing(self, *args):
pass
def _pixelsToUnits(self, srcPos):
srcPos = self._toVector(srcPos)
src = np.float32([[srcPos.x, srcPos.y]])
src = np.array([src])
out = cv2.perspectiveTransform(src, self.p2uTranformMatrix)
return Vector2(int(out[0][0][0]), int(out[0][0][1]))
def _unitsToPixels(self, srcPos):
srcPos = self._toVector(srcPos)
src = np.float32([[srcPos.x, srcPos.y]])
src = np.array([src])
out = cv2.perspectiveTransform(src, self.u2pTranformMatrix)
return Vector2(int(out[0][0][0]), int(out[0][0][1]))
def _toTuple(self, vector):
if isinstance(vector, Vector2):
return (int(vector.x), int(vector.y))
else:
return (int(vector[0]), int(vector[1]))
def _toVector(self, vector):
if isinstance(vector, Vector2):
return Vector2(int(vector.x), int(vector.y))
else:
return Vector2(int(vector[0]), int(vector[1]))
def _createTransformMatrices(self, fieldCorners):
if self.prevFieldCorners is None or not (np.all(
self.prevFieldCorners == fieldCorners)):
# print("Calculating transform matrices.")
self.prevFieldCorners = fieldCorners.copy()
source = np.float32([[
point[0] * self.settings["resolution"][0],
point[1] * self.settings["resolution"][1]
] for point in self.settings["fieldCorners"].tolist()])
dst = np.float32([[0, -FIELD_HEIGHT / 2],
[FIELD_WIDTH, -FIELD_HEIGHT / 2],
[FIELD_WIDTH, FIELD_HEIGHT / 2],
[0, FIELD_HEIGHT / 2]])
dst = np.array([dst])
self.p2uTranformMatrix = cv2.getPerspectiveTransform(source, dst)
self.u2pTranformMatrix = cv2.getPerspectiveTransform(dst, source)
def _writeText(self,
text,
position=(10, 30),
fontScale=1,
fontColor=(255, 255, 255)):
font = cv2.FONT_HERSHEY_SIMPLEX
lineType = 2
cv2.putText(self.frame, text, self._toTuple(position), font, fontScale,
fontColor, lineType)
def _drawLine(self,
startPoint=(0, 10),
endPoint=(250, 10),
color=(0, 255, 0),
thickness=2):
self.frame = cv2.line(self.frame, self._toTuple(startPoint),
self._toTuple(endPoint), color, thickness)
def _lineHalf(self, startPoint, endPoint):
x = round((startPoint[0] + endPoint[0]) / 2)
y = round((startPoint[1] + endPoint[1]) / 2)
return (x, y)
def _drawPoint(self, center, color=(0, 255, 255), size=5):
center = self._toTuple(center)
cv2.circle(self.frame, center, size, color, -1)
def _drawPuck(self, center, color=(0, 0, 255)):
center = self._toTuple(center)
cv2.circle(self.frame, center, PUCK_RADIUS, color, 1)
cv2.circle(self.frame, center, 2, color, -1)
def _drawField(self, npPoints, color=(0, 255, 0), thickness=3):
points = [self._toTuple(p) for p in npPoints]
for i in range(len(points) - 1):
self._drawLine(points[i], points[i + 1])
self._drawLine(points[3], points[0])
# Draw field center
self._drawLine(self._lineHalf(points[0], points[1]),
self._lineHalf(points[2], points[3]),
thickness=1)
self._drawLine(self._lineHalf(points[1], points[2]),
self._lineHalf(points[3], points[0]),
thickness=1)
def _mouseHSV(self, event, x, y, flags, param):
self.cursorPosition = Vector2(x, y)
class Camera(): def __init__(self, game, fps): self.game = game self.filter = Filter(15, 2.2, 1.2) self.delay = 0.02 # in seconds - will not be precise self.frameRate = fps self.stepsSinceLastCapture = 0 self.newData = False self.puckPosition = Vector2(0, 0) self.positionHistory = [] self.xGrid = [] self.yGrid = [] self.createGrid(4) self.historySize = max(round(self.delay/(1/self.frameRate)),1) def update(self): stepsToCapture = round((1/self.frameRate)/self.game.simulation.stepTime) if self.stepsSinceLastCapture >= stepsToCapture: self.positionHistory.insert(0,self.capturePuck()) if(len(self.positionHistory) > self.historySize): self.positionHistory.pop(-1) self.puckPosition = self.positionHistory[-1] self.newData = True self.stepsSinceLastCapture = 0 self.stepsSinceLastCapture += 1 def createGrid(self, step): self.xGrid.append(0) i = 0 while self.xGrid[i] < FIELD_WIDTH: self.xGrid.append(self.xGrid[i] + step) i += 1 self.yGrid.append(-FIELD_HEIGHT/2) i = 0 while (self.yGrid[i] < FIELD_HEIGHT/2): self.yGrid.append(self.yGrid[i] + step) i += 1 def capturePuck(self): self.puckPosition = Vector2(self.game.simulation.puck.position) self.puckPosition.x += gauss(0, 2) self.puckPosition.y += gauss(0, 2) self.blockView() self.discretize() self.puckPosition = self.filter.filterData(self.puckPosition) return self.puckPosition def discretize(self): tempPos = 0 for element in self.xGrid: if (abs(self.puckPosition.x - element) < abs(self.puckPosition.x - tempPos)): tempPos = element self.puckPosition.x = tempPos tempPos = 0 for element in self.yGrid: if (abs(self.puckPosition.y - element) < abs(self.puckPosition.y - tempPos)): tempPos = element self.puckPosition.y = tempPos def blockView(self): for striker in self.game.simulation.strikers: dist = self.puckPosition.x - striker.position.x if(abs(dist) < PUCK_RADIUS*1.5): self.puckPosition.x = striker.position.x + sign(dist) * PUCK_RADIUS + 0.5*(dist) break
class BaseStrategy():
def __init__(self):
self.description = "Strategy with no gameplay mechanics."
# DEBUG
self.debugLines = []
self.debugPoints = []
self.debugString = ""
# Striker
self.striker = StrategyStriker()
self.opponentStriker = StrategyStriker()
# Striker Limits
self.maxSpeed = MAX_SPEED
self.acceleration = MAX_ACCELERATION
self.deceleration = MAX_DECELERATION
self.gain = KP_GAIN
# Puck
self.puck = StrategyPuck()
self.puckHistory = []
self.puckHistory.append(self.puck)
self.lastMove = 0
# Trajectory info
self.goalLineIntersection = 0
self.willBounce = False
# Parameters
self.historySize = 50
self.noOfBounces = 1
self.minSpeedLimit = 100
self.highAngleTolerance = 50
self.mediumAngleTolerance = 15
self.lowAngleTolerance = 8
self.positionTolerance = 100
self.capturesWithBadLowAngle = 0
self.capturesWithBadMediumAngle = 0
# States
self.stepTime = 0
self.gameTime = 0
self.timeSinceLastCameraInput = 0
self.sameCameraInputsInRow = 0
self.previousErrorSide = 0
self.firstUsefull = 1
self.predictedPosition = Vector2(0, 0)
# Filter
self.angleFilter = Filter(15, 2, 1, isVector=False)
# Init
for i in range(self.historySize - 1):
self.puckHistory.append(StrategyPuck())
# Main process function -----------------------------------------------------------------------------------
def process(self, stepTime):
# DEBUG
self.debugLines = []
self.debugPoints = []
# self.debugString = ""
self.stepTick(stepTime)
self._process()
self.moveIfStuck()
self.limitMovement()
self.calculateDesiredVelocity()
# Only this should be overwriten in inherited strategies
def _process(self):
self.setDesiredPosition(self.striker.position) # Placeholder
# Your strategy code
# Puck position handlers ------------------------------------------------------------------
def stepTick(self, stepTime):
self.stepTime = stepTime
self.gameTime += stepTime
for puck in self.puckHistory:
puck.timeSinceCaptured += stepTime
def cameraInput(self, pos):
if pos == self.puck.position: return
self.initialCheck(pos)
self.setPuck(pos)
self.checkState()
self.calculateTrajectory()
def initialCheck(self, pos):
currentStepVector = pos - self.puck.position
stepDistance = currentStepVector.magnitude()
if self.puck.timeSinceCaptured == 0:
stepSpeed = 0
else:
stepSpeed = stepDistance / self.puck.timeSinceCaptured
errorAngle = self.getAngleDifference(currentStepVector,
self.puck.velocity)
# Low angle condition
if abs(errorAngle) > self.lowAngleTolerance and sign(
errorAngle) == self.previousErrorSide:
self.capturesWithBadLowAngle += 1
if (self.capturesWithBadLowAngle > 4):
# print("Low Angle error")
for i in range(4):
self.puckHistory[self.firstUsefull].state = USELESS
if self.firstUsefull > 1: self.firstUsefull -= 1
else:
self.capturesWithBadLowAngle = 0
self.previousErrorSide = sign(errorAngle)
if stepSpeed > 200 and stepDistance > 4 and abs(errorAngle):
# Medium angle condition
if abs(errorAngle) > self.mediumAngleTolerance and sign(
errorAngle) == self.previousErrorSide:
self.capturesWithBadMediumAngle += 1
if (self.capturesWithBadMediumAngle > 3):
# print("Low angle condition.. 4 states -> useless")
self.capturesWithBadLowAngle = 0
self.capturesWithBadMediumAngle = 0
# print("Medium Angle error")
for i in range(3, len(self.puckHistory)):
self.puckHistory[i].state = USELESS
else:
self.capturesWithBadMediumAngle = 0
# Debug
# if len(self.puck.trajectory) > 0:
# trajectoryLine = Line(self.puckHistory[self.firstUsefull].position, self.puck.position)
# bounceLine = Line(Vector2(0, sign(pos.y) * (FIELD_HEIGHT/2 - PUCK_RADIUS)), Vector2(FIELD_WIDTH, sign(pos.y) * (FIELD_HEIGHT/2 - PUCK_RADIUS)))
# self.debugLines.append(trajectoryLine)
# self.debugLines.append(bounceLine)
# self.debugPoints.append(self.getIntersectPoint(trajectoryLine, bounceLine))
# High angle condition
if (abs(errorAngle) > self.highAngleTolerance) or (
stepSpeed > 700 and stepDistance > 25
and abs(errorAngle) > self.highAngleTolerance * .4):
self.capturesWithBadLowAngle = 0
self.capturesWithBadMediumAngle = 0
# print("Angle condition: " + str(errorAngle))
if abs(pos.y) > max(
200, FIELD_HEIGHT / 2 -
(stepDistance * abs(self.puck.vector.y) + PUCK_RADIUS)
) and sign(currentStepVector.x) == sign(
self.puck.velocity.x
) and sign(self.puck.velocity.y) == sign(
pos.y
) and self.puck.state == ACURATE: # seems like bounce from sidewalls occured
trajectoryLine = Line(
self.puckHistory[self.firstUsefull].position,
self.puck.position)
bounceLine = Line(
Vector2(0,
sign(pos.y) *
(FIELD_HEIGHT / 2 - PUCK_RADIUS)),
Vector2(FIELD_WIDTH,
sign(pos.y) *
(FIELD_HEIGHT / 2 - PUCK_RADIUS)))
bouncePoint = trajectoryLine.getIntersectPoint(bounceLine)
self.debugLines.append(trajectoryLine)
self.debugLines.append(bounceLine)
bouncePoint = trajectoryLine.getIntersectPoint(bounceLine)
self.puck.position = bouncePoint
# print(bouncePoint)
for i in range(len(self.puckHistory)):
self.puckHistory[i].state = USELESS
# print("High Angle error: " + str(abs(errorAngle)))
# Quick acceleration - does nothing for now
# i = self.firstUsefull - 1
# while self.puckHistory[firstUsefull].position.distance_squared_to(self.puckHistory[i].position) < positionTolerance**2:
# if i <= 1: break
# i -= 1
def setStriker(self, pos, velocity=None):
if velocity == None:
step = pos - self.striker.position
velocity = Vector2(step)
stepMag = step.magnitude()
if stepMag > 0.001:
if self.stepTime == 0:
velocity.scale_to_length(0)
else:
velocity.scale_to_length(stepMag / self.stepTime)
self.striker.velocity = Vector2(velocity)
self.striker.position = Vector2(pos)
def setOpponentStriker(self, pos, velocity=None):
if velocity == None:
step = pos - self.opponentStriker.position
velocity = Vector2(step)
stepMag = step.magnitude()
if stepMag > 0.001:
if self.stepTime == 0:
velocity.scale_to_length(0)
else:
velocity.scale_to_length(stepMag / self.stepTime)
self.striker.velocity = Vector2(velocity)
self.opponentStriker.position = Vector2(pos)
def setPuck(self, pos):
self.puck = StrategyPuck(ACURATE, pos)
self.puckHistory.pop(-1)
self.puckHistory.insert(0, self.puck)
self.firstUsefull = len(self.puckHistory) - 1
while (self.puckHistory[self.firstUsefull].state == USELESS):
self.firstUsefull -= 1
if self.firstUsefull == 1: break
# print(self.firstUsefull)
if not self.puckHistory[self.firstUsefull].timeSinceCaptured == 0:
# if self.firstUsefull > 3:
stepVector = pos - self.puckHistory[self.firstUsefull].position
self.puck.velocity = stepVector / self.puckHistory[
self.firstUsefull].timeSinceCaptured
# Filter velocity and normal vector
(r, fi) = self.puck.velocity.as_polar()
fi = self.angleFilter.filterData(fi, cyclic=360)
self.puck.velocity.from_polar((r, fi if fi <= 180 else fi - 360))
# print("-----")
# print(fi)
# print(r)
# self.puck.velocity = self.velocityFilter.filterData(self.puck.velocity)
self.puck.vector = self.puck.velocity.normalize()
self.puck.speedMagnitude = r
self.puck.angle = fi if fi > 0 else 360 - abs(fi)
# else:
# self.puck.state = INACURATE
self.puck.timeSinceCaptured = 0
def checkState(self):
# Check for inacurate
if abs(self.puck.speedMagnitude < self.minSpeedLimit):
self.puck.state = INACURATE
# if abs(self.puck.vector.y) > 0.9:
# self.puck.state = INACURATE
if self.puck.speedMagnitude < self.minSpeedLimit * 5 and self.firstUsefull < min(
3, round(self.historySize / 20)):
self.puck.state = INACURATE
# Desired position / velocity modification -------------------------------------------------------------------
def setDesiredPosition(self, pos):
self.striker.desiredPosition = Vector2(pos)
self.limitMovement()
self.calculateDesiredVelocity()
def setDesiredVelocity(self, vel):
posNextStep = self.striker.position + vel * self.stepTime
if posNextStep.x > STRIKER_AREA_WIDTH:
vel.x = 0
if abs(posNextStep.y) > YLIMIT:
vel.y = 0
if posNextStep.x < XLIMIT:
vel.x = 0
self.striker.desiredVelocity = vel
def clampDesired(self, fromPos, step):
desiredPos = fromPos + step
line = Line(fromPos, desiredPos)
self.debugLines.append(line)
if desiredPos.x > STRIKER_AREA_WIDTH:
desiredPos = line.getBothCoordinates(x=STRIKER_AREA_WIDTH)
if abs(desiredPos.y) > YLIMIT:
desiredPos = line.getBothCoordinates(y=sign(desiredPos.y) * YLIMIT)
if desiredPos.x < XLIMIT:
desiredPos = line.getBothCoordinates(x=XLIMIT)
self.setDesiredPosition(desiredPos)
def limitMovement(self):
if self.striker.desiredPosition.x > STRIKER_AREA_WIDTH:
self.striker.desiredPosition.x = STRIKER_AREA_WIDTH
if abs(self.striker.desiredPosition.y) > YLIMIT:
self.striker.desiredPosition.y = sign(
self.striker.desiredPosition.y) * YLIMIT
if self.striker.desiredPosition.x < XLIMIT:
self.striker.desiredPosition.x = XLIMIT
# Check if near corner
if self.striker.desiredPosition.x < CORNER_SAFEGUARD_X:
if abs(self.striker.desiredPosition.y
) > FIELD_HEIGHT / 2 - CORNER_SAFEGUARD_Y:
self.striker.desiredPosition.y = sign(
self.striker.desiredPosition.y) * (
FIELD_HEIGHT / 2 - (STRIKER_RADIUS + PUCK_RADIUS * 2))
# Check if near goal
if GOAL_SPAN / 2 - GOAL_CORNER_SAFEGUARD_Y < abs(
self.striker.desiredPosition.y) < GOAL_SPAN / 2:
if self.striker.desiredPosition.x < GOAL_CORNER_SAFEGUARD_X:
self.striker.desiredPosition.x = GOAL_CORNER_SAFEGUARD_X
def calculateDesiredVelocity(self):
# self.striker.desiredVelocity = self.gain*(self.striker.desiredPosition - self.striker.position)
# speedDiff = abs(self.striker.velocity.x) - abs(self.striker.velocity.y)
# maxSpeed = max(abs(self.striker.velocity.x), abs(self.striker.velocity.y), self.maxSpeed/10)
# xmag = max(abs(self.striker.velocity.x),self.maxSpeed/10)/maxSpeed
# ymag = max(abs(self.striker.velocity.y),self.maxSpeed/10)/maxSpeed
# self.striker.desiredVelocity.x = xmag * self.gain*(self.striker.desiredPosition.x - self.striker.position.x)
# self.striker.desiredVelocity.y = ymag * self.gain*(self.striker.desiredPosition.y - self.striker.position.y)
self.striker.desiredVelocity.x = self.gain * (
self.striker.desiredPosition.x - self.striker.position.x)
self.striker.desiredVelocity.y = self.gain * (
self.striker.desiredPosition.y - self.striker.position.y)
# if oppositeSigns(self.striker.desiredVelocity.x, self.striker.velocity.x):
# self.striker.desiredVelocity.x = 10 * self.gain*(self.striker.desiredPosition.x - self.striker.position.x)
# if oppositeSigns(self.striker.desiredVelocity.y, self.striker.velocity.y):
# self.striker.desiredVelocity.y = 10 * self.gain*(self.striker.desiredPosition.y - self.striker.position.y)
# Checkers ------------------------------------------------------------------------------
def isOutsideLimits(self, pos):
if pos.x > STRIKER_AREA_WIDTH: return True
if abs(pos.y) > YLIMIT: return True
if pos.x < XLIMIT: return True
if pos.x > FIELD_WIDTH - XLIMIT: return True
return False
def isPuckOutsideLimits(self, pos):
if pos.x > STRIKER_AREA_WIDTH: return True
if abs(pos.y) > FIELD_HEIGHT / 2 - PUCK_RADIUS * 0.8: return True
if pos.x < PUCK_RADIUS * 0.8: return True
if pos.x > FIELD_WIDTH - PUCK_RADIUS * 0.8: return True
return False
def isPuckBehingStriker(self, pos=None):
if pos is None: pos = self.puck.position
return self.striker.position.x > pos.x - PUCK_RADIUS * 2
# Get functions --------------------------------------------------------------
def getAngleDifference(self, vector1, vetor2):
errorAngle = vector1.angle_to(vetor2)
if abs(errorAngle) > 180: errorAngle -= sign(errorAngle) * 360
return errorAngle
def getPredictedPuckPosition(self, strikerPos=None, reserve=1.3):
if strikerPos is None: strikerPos = self.striker.desiredPosition
if self.puck.state == INACURATE:
self.predictedPosition = Vector2(self.puck.position)
return Vector2(self.puck.position)
if len(self.puck.trajectory) > 0:
try:
step = strikerPos - self.striker.position
dist = step.magnitude()
# Compute time, that will take striker to move to desired position
a = getSpeedInXYdir(
step.x, step.y, self.acceleration).magnitude(
) # Acceleration in direction to desired pos
vm = getSpeedInXYdir(step.x, step.y, self.maxSpeed).magnitude(
) # Max velocity in direction to desired pos
v0 = sign(self.striker.velocity.dot(step)) * (
step * self.striker.velocity.dot(step) /
step.dot(step)).magnitude(
) # Projected current velocity in direction to desired pos
#(how fast the striker is moving in the right direction)
t1 = (
vm - v0
) / a # Time it would take for striker to accelerate to max speed in the direction to desired pos
d1 = 1 / 2 * a * t1**2 + v0 * t1 # Distance the striker would cover in t1 time in direction to desiered pos
if d1 > dist: # if the "would be" distance is greater than actual distance to desired pos then:
time = max(
(-v0 + (v0**2 + 2 * a * dist)**.5) / a,
(-v0 - (v0**2 + 2 * a * dist)**.5) / a
) # Calculate time to travel that distance with good old kinematic formula Δx=1/2at^2 + v0t
else: # else:
time = t1 + (
dist - d1
) / vm # Get the calculated t1 time and add time calculated as (residual distance)/maximum velocity
# time = dist/vm
vector = Vector2(
self.puck.vector) * (self.puck.speedMagnitude * time)
position = self.puck.position + vector * reserve
if position.x < PUCK_RADIUS and abs(
position.y) < FIELD_HEIGHT - PUCK_RADIUS:
position.x = PUCK_RADIUS
position.y = self.goalLineIntersection
self.predictedPosition = position
return position
except:
return Vector2(0, 0)
# Line math ---------------
def calculateTrajectory(self):
self.puck.trajectory = []
yBound = (FIELD_HEIGHT / 2 - PUCK_RADIUS)
myLine = Line(self.puck.position, self.puck.position)
tempVector = Vector2(self.puck.vector)
self.goalLineIntersection = -10000
for i in range(self.noOfBounces + 1):
if not tempVector.x == 0:
a = tempVector.y / tempVector.x
b = myLine.start.y - a * myLine.start.x
else:
a = 0
b = 0
if tempVector.x == 0: # not a function - vertical line
myLine.end.x = myLine.start.x
myLine.end.y = sign(tempVector.y) * yBound
elif a == 0: # no slope - horizontal line
myLine.end.x = sign(tempVector.x) * FIELD_WIDTH
myLine.end.y = myLine.start.y
else: # normal linear line
myLine.end.x = (sign(tempVector.y) * yBound - b) / a
myLine.end.y = sign(tempVector.y) * yBound
tempVector.y *= -1
if myLine.end.x < PUCK_RADIUS:
myLine.end.x = PUCK_RADIUS
myLine.end.y = a * myLine.end.x + b
tempVector.x *= -1
tempVector.y *= -1
# Set goal interection
self.goalLineIntersection = myLine.end.y
elif myLine.end.x > FIELD_WIDTH - PUCK_RADIUS:
myLine.end.x = FIELD_WIDTH - PUCK_RADIUS
myLine.end.y = a * myLine.end.x + b
tempVector.x *= -1
tempVector.y *= -1
self.puck.trajectory.append(myLine.copy())
# If puck aims at goal, break
if abs(myLine.end.y) < FIELD_HEIGHT / 2 - PUCK_RADIUS: break
myLine.start.x = myLine.end.x
myLine.start.y = myLine.end.y
if len(self.puck.trajectory) > 1:
self.willBounce = True
else:
self.willBounce = False
# Basic strategy functions used in Process method ---------------------------------------------
def defendGoalDefault(self):
if self.willBounce and self.puck.state == ACURATE\
and (self.puck.vector.x < -0.5 or (self.puck.vector.x < 0 and self.puck.trajectory[-1].end.x <= PUCK_RADIUS))\
and not (self.puck.position.x > FIELD_WIDTH*.6 and self.puck.speedMagnitude < 500):
if self.puck.trajectory[-1].end.x > XLIMIT + STRIKER_RADIUS:
fromPoint = self.puck.trajectory[-1].end
else:
fromPoint = self.puck.trajectory[-1].start
else:
fromPoint = self.puck.position
a = Line(fromPoint, Vector2(0, 0))
b = Line(Vector2(DEFENSE_LINE, -FIELD_HEIGHT / 2),
Vector2(DEFENSE_LINE, FIELD_HEIGHT / 2))
desiredPosition = a.getIntersectPoint(b)
self.debugLines.append(a)
self.debugLines.append(b)
self.debugString = "basic.defendGoalDefault"
if desiredPosition is not None:
self.setDesiredPosition(Vector2(desiredPosition))
def defendGoalLastLine(self):
if self.puck.position.x < self.striker.position.x and abs(
self.puck.position.y
) < GOAL_SPAN * .7: # if puck is behind striker and is infront of goal
self.setDesiredPosition(
Vector2(self.striker.position.x,
GOAL_SPAN / 2 * -sign(self.puck.position.y)))
if abs(
self.striker.position.y -
GOAL_SPAN / 2 * -sign(self.puck.position.y)
) < CLOSE_DISTANCE or self.striker.position.x < XLIMIT + CLOSE_DISTANCE:
self.setDesiredPosition(
Vector2(XLIMIT,
GOAL_SPAN / 2 * -sign(self.puck.position.y)))
if self.striker.position.x < XLIMIT + CLOSE_DISTANCE:
self.setDesiredPosition(
Vector2(XLIMIT, self.puck.position.y))
return
if self.striker.position.x < self.puck.position.x - PUCK_RADIUS < self.striker.position.x + PUCK_RADIUS + STRIKER_RADIUS: # if puck is just next to striker
blockY = self.puck.position.y
elif not self.goalLineIntersection == -10000 and self.puck.state == ACURATE and self.puck.vector.x < 0:
if self.puck.vector.x > -.7:
self.defendGoalDefault()
return
else:
blockY = self.goalLineIntersection
elif self.puck.state == ACURATE and self.puck.vector.x < 0:
blockY = self.puck.trajectory[0].end.y
else:
blockY = self.puck.position.y
# self.debugLines.append(a)
self.debugString = "basic.defendGoalLastLine"
self.setDesiredPosition(
Vector2(
XLIMIT,
sign(blockY) *
min(GOAL_SPAN / 2 + STRIKER_RADIUS, abs(blockY))))
# self.setDesiredPosition(Vector2(XLIMIT, sign(self.puck.position.y) * min(GOAL_SPAN/2, abs(self.puck.position.y))))
def defendTrajectory(self):
if len(self.puck.trajectory) > 0:
vector = Vector2(-self.puck.vector.y, self.puck.vector.x)
secondPoint = self.striker.position + vector
self.debugString = "basic.defendTrajectory"
self.debugLines.append(self.puck.trajectory[0])
self.debugLines.append(Line(self.striker.position, secondPoint))
self.setDesiredPosition(self.puck.trajectory[0].getIntersectPoint(
Line(self.striker.position, secondPoint)))
def moveIfStuck(self):
if self.puck.speedMagnitude > 100 or self.puck.position.x > STRIKER_AREA_WIDTH + PUCK_RADIUS * .8:
self.lastMove = self.gameTime
if 3 < self.gameTime - self.lastMove < 5:
self.setDesiredPosition(self.puck.position)
def shouldIntercept(self):
if len(self.puck.trajectory) == 0:
return 0
return self.puck.state == ACURATE and (
not self.willBounce or
(sign(self.puck.vector.y) * self.puck.trajectory[-1].end.y >
GOAL_SPAN)) and self.puck.vector.x < 0
def isPuckDangerous(self):
if self.puck.position.x > STRIKER_AREA_WIDTH:
return True
if abs(self.puck.velocity.y) > self.maxSpeed:
return True
if self.willBounce:
return True
if self.striker.position.x > self.puck.position.x - PUCK_RADIUS:
return True
if abs(self.goalLineIntersection) < (
GOAL_SPAN / 2) * 1.2 and self.puck.state == ACURATE:
if len(self.puck.trajectory) > 0:
if self.puck.trajectory[-1].getPointLineDist(
self.striker.position) > PUCK_RADIUS:
return True
return False