def __init__(self):
self.currentPoint = Point(-100, -100) # prevent crashing
self.pointHistory = [Point(-100, -100)]
# speed is in xm per tick
self.currentSpeed = None
self.speedHistory = []
# note that direction corresponds to direction of movement
# and not necessarily the direction the object is facing
self.direction = None
# acceleration is in cm per tick per tick
self.acceleration = None
def __init__(self, debug=False):
# Actions queued to simulate.
self.currentActionQueue = []
self.grabbed = True
self.holdingBall = False
self.lastCommandFinished = 0
self.lastCommandSent = 0
simulatedStart(Point(50, 50), Point(200, 25), Point(166, 211),
Point(52, 102), 15, 38, 150, -62, Point(100, 100),
BallStatus.free)
# since we're using a simulator, set the fact that we know where we are exactly at all times
POINT_ACCURACY = 0.1
ANGLE_ACCURACY = 0.1
def predictedPosition(self, t):
"""Calculate the position of the object in t ticks
if it continues accelerating at its current rate
Args:
t (int): the number of ticks into the future you're predicting the object's position.
Note that the large t gets, the less accurate the prediction will be.
Returns:
Point of the predicted position of the object
"""
try:
displacement = self.currentSpeed * t + 0.5 * self.acceleration * (
t**2)
xDisplacement = round(sin(self.direction) * displacement, 2)
yDisplacement = round(cos(self.direction) * displacement, 2)
return Point(self.currentPoint.x + xDisplacement,
self.currentPoint.y + yDisplacement)
except TypeError:
if isinstance(t, int):
return self.currentPoint
else:
raise TypeError("Float expected, " + t.__class__.__name__ +
" found")
def update(self, newPoint):
"""Update the object with the position it's in this new tick.
To be called every tick.
Args:
newPoint (Point): the new coordinates of this object
"""
# if we've temporarily lost the object, assume it's in the same place
if newPoint is None:
# TODO: assume it's moved on slightly
newPoint = self.currentPoint
robot = (self.name if self.name is not None else "moveable")
print("Lost position of " + robot)
# if we've yet to find the robot, don't bother updating anything
if newPoint is not None:
# save the old point
if self.currentPoint is not Point(-100, -100):
self.pointHistory.append(self.currentPoint)
# save the new position
self.currentPoint = newPoint
# only store a max of _HISTORY_SIZE points in the history
if len(self.pointHistory) > self._HISTORY_SIZE:
self.pointHistory.pop(0)
try:
# calculate and save the current direction
self.direction = self.pointHistory[0].bearing(
self.pointHistory[-1])
except IndexError:
# if we don't have enough information to
# calculate a direction so wait for now
pass
# calculate and save the new speed
try:
newSpeed = self.pointHistory[0].distance(
self.pointHistory[-1]) / (len(self.pointHistory) - 1)
newSpeed = round(newSpeed, 2)
self.speedHistory.append(newSpeed)
self.currentSpeed = newSpeed
# only store a max of _HISTORY_SIZE-1 points in the history
if len(self.speedHistory) > self._HISTORY_SIZE - 1:
self.speedHistory.pop(0)
except (ZeroDivisionError, IndexError):
pass
# calculate and save the new acceleration
try:
acceleration = (self.speedHistory[-1] - self.speedHistory[0]
) / (len(self.speedHistory) - 1)
self.acceleration = round(acceleration, 3)
except (ZeroDivisionError, IndexError):
# if we don't have enough data to calculate an
# acceleration (not enough speeds recorded), wait for now
pass
def ghost_walk(angle_to_face):
ghost_flag = False
for i in range(0, 10):
ghost_pt_x = random.random() * 30 * random.choice([1, -1])
ghost_pt_y = random.random() * 30 * random.choice([1, -1])
target = me.currentPoint.x + ghost_pt_y, me.currentPoint.y + ghost_pt_y
if target[0] > 0 and target[0] < PITCH_LENGTH and target[
1] > 0 and target[1] < PITCH_WIDTH and not isEnemyBox(
Point(target[0], target[1])) and lineOfSight(
me.currentPoint, Point(target[0], target[1])):
ghost_flag = True
break
if ghost_flag:
angle_to_move = me.bearing(Point(target[0], target[1]))
print "Ghost"
controller.move(angle_to_face, angle_to_move, 60, None, None)
else:
print "None"
controller.stop_robot()
def guardGoal():
"""Go to the goal line and face the enemy with the ball"""
me.goal = Goals.guardGoal
angle_to_face = me.bearing(ball)
# if not on our goal line, move into the middle of it
if abs(me.currentPoint.x - ourGoal.x) > 15:
angle_to_move = me.bearing(ourGoal)
distance = me.distance(ourGoal)
controller.move(angle_to_move, angle_to_move, distance)
# if we're on our goal line, move to block the ball
else:
# work out which enemy has the ball
if ball.status == BallStatus.enemyA:
enemyNum = 0
elif ball.status == BallStatus.enemyB:
enemyNum = 1
else:
print("just guarding lol")
controller.move(angle_to_face, 0, 0, False, rotate_in_place=True)
return
# calculate where on the y axis the ball would hit if kicked now
x_dist = enemies[enemyNum].currentPoint.x - ourGoal.x
y_dist = x_dist * tan(enemies[enemyNum].currentRotation)
y_intersection = enemies[enemyNum].currentPoint.y + y_dist
# calculate where we should therefore go to (we don't want to leave the goal line)
minY = PITCH_WIDTH / 2 - 0.5 * GOAL_WIDTH + ROBOT_WIDTH / 2
maxY = PITCH_WIDTH / 2 + 0.5 * GOAL_WIDTH - ROBOT_WIDTH / 2
point_to_be = Point(ourGoal.x, max(minY, min(maxY, y_intersection)))
# if we're not where we should be, move there holonomically
if not nearEnough(point_to_be, me.currentPoint, near_enough_point=10):
# turn to face the opposite side of the pitch before the holo movement
if OUR_GOAL == 'left':
angle_to_face = 0
else:
angle_to_face = 180
# calculate the parameters to send to the robot
distance = me.distance(point_to_be)
# we want to move holonomically up and down
if point_to_be.y < me.currentPoint.y:
angle_to_move = 90
else:
angle_to_move = -90
controller.move(angle_to_face, angle_to_move, distance)
# if we're in position already but just facing wrongly, turn to face the robot with the ball
elif not nearEnough(angle_to_face, me.currentRotation):
controller.move(angle_to_face, 0, 0, False, rotate_in_place=True)
# if we're all set, just wait for something to happen
else:
controller.stop_robot()
def blockPass():
"""Move to inbetween the two enemies"""
me.goal = Goals.blockPass
# work out where to move to
e0 = enemies[0].currentPoint
e1 = enemies[1].currentPoint
x = (e0.x + e1.x) / 2
y = (e0.y + e1.y) / 2
point_to_be = Point(x, y)
# work out the parameters for the move command
angle_to_face = point_to_be.bearing(ball.currentPoint)
angle_to_move = me.bearing(point_to_be)
distance = me.distance(point_to_be)
if not isEnemyBox(point_to_be):
controller.move(angle_to_move, angle_to_move, distance)
else:
ghost_walk(angle_to_face)
def move(self):
if self.currentSpeed == 0 & self.acceleration == 0:
return
self.currentSpeed = self.currentSpeed + self.acceleration * tickTimeLeft
distanceTravelled = self.currentSpeed * tickTimeLeft
angle = self.currentRotation
xDisplacement = round(cos(angle) * distanceTravelled, 2)
yDisplacement = -round(sin(angle) * distanceTravelled, 2)
newX = self.currentPoint.x + xDisplacement
newY = self.currentPoint.y + yDisplacement
topWall = xDisplacement == PITCH_WIDTH
leftWall = xDisplacement == 0
rightWall = yDisplacement == PITCH_LENGTH
bottomWall = yDisplacement == 0
if topWall | leftWall | rightWall | bottomWall:
self.bounce(topWall, leftWall, rightWall, bottomWall)
self.currentPoint = Point(newX, newY)
if self.currentSpeed < 0:
self.currentSpeed = 0
self.acceleration = 0
# our supreme robot
me = Robot(name="me")
me.grabbed = True
# Team 3's robot
ally = Robot(name="ally")
# the two robots we're against
enemies = [Robot(name="pinkEnemy"), Robot(name="greenEnemy")]
# a list containing all robots on the field for convenience
robots = [me, ally] + enemies
# guess what this could possibly be
ball = Ball(name="ball")
#list of all movables
moveables = [me, ally, ball] + enemies
# the point at the center of the goal
leftGoalCenter = Point(30, PITCH_WIDTH / 2)
rightGoalCenter = Point(PITCH_LENGTH - 30, PITCH_WIDTH / 2)
if OUR_GOAL == 'right':
ourGoal = rightGoalCenter
opponentGoal = leftGoalCenter
else:
ourGoal = leftGoalCenter
opponentGoal = rightGoalCenter
def lineOfSight(From, To):
dxc = From.x - To.x
dyc = From.y - To.y
for obj in moveables:
def tick(self, tickTimeLeft=TICK_TIME):
'''Update the status of our robot and the ball based on our recent actions
tickTimeLeft: the time remaining before the next tick, TICK_TIME by default
or slightly less than this if an action finishes halfway through a tick
and this gets called again on the remaining time'''
try:
currentAction = self.currentActionQueue[0]
except IndexError:
# if it's not currently carrying out an action, it'll stay the same
return
if currentAction['action'] == SimulatorActions.moveHolo:
if TICK_TIME < currentAction['timeLeft']:
currentAction['timeLeft'] -= TICK_TIME
distanceTravelled = MAX_SPEED * TICK_TIME * 0.9
angle = simulatedMe.currentRotation + currentAction['action'][0]
xDisplacement = round(cos(angle) * distanceTravelled, 2)
yDisplacement = -round(sin(angle) * distanceTravelled, 2)
simulatedMe.currentPoint = Point(
simulatedMe.currentPoint.x + xDisplacement,
simulatedMe.currentPoint.y + yDisplacement)
else:
tickTimeLeft = TICK_TIME - currentAction['timeLeft']
distanceTravelled = MAX_SPEED * currentAction['timeLeft'] * 0.9
angle = simulatedMe.currentRotation + currentAction['action'][0]
xDisplacement = round(cos(angle) * distanceTravelled, 2)
yDisplacement = -round(sin(angle) * distanceTravelled, 2)
simulatedMe.currentPoint = Point(
simulatedMe.currentPoint.x + xDisplacement,
simulatedMe.currentPoint.y + yDisplacement)
# report that we've finished executing this command
self.currentActionQueue.pop(0)
self.lastCommandFinished += 1
# start the next action if it's queued
self.tick(tickTimeLeft)
# if currently moving forwards
if currentAction['action'] == SimulatorActions.moveForwards:
# if it'll keep going for this whole tick, move the simulated robot forwards the appropriate amount
if TICK_TIME < currentAction['timeLeft']:
currentAction['timeLeft'] -= TICK_TIME
distanceTravelled = MAX_SPEED * TICK_TIME
angle = simulatedMe.currentRotation
xDisplacement = round(cos(angle) * distanceTravelled, 2)
yDisplacement = -round(sin(angle) * distanceTravelled, 2)
simulatedMe.currentPoint = Point(
simulatedMe.currentPoint.x + xDisplacement,
simulatedMe.currentPoint.y + yDisplacement)
# if not, move the simulated robot forwards the lesser amount, then start the next action in the queue with the remaining time
else:
tickTimeLeft = TICK_TIME - currentAction['timeLeft']
distanceTravelled = MAX_SPEED * currentAction['timeLeft']
angle = simulatedMe.currentRotation
xDisplacement = round(cos(angle) * distanceTravelled, 2)
yDisplacement = -round(sin(angle) * distanceTravelled, 2)
simulatedMe.currentPoint = Point(
simulatedMe.currentPoint.x + xDisplacement,
simulatedMe.currentPoint.y + yDisplacement)
# report that we've finished executing this command
self.currentActionQueue.pop(0)
self.lastCommandFinished += 1
# start the next action if it's queued
self.tick(tickTimeLeft)
# if currently rotating
elif currentAction['action'] == SimulatorActions.rotate:
# if it'll keep going for this whole tick, turn the simulated robot forwards the appropriate amount
if TICK_TIME < currentAction['timeLeft']:
# calculate how far to turn
currentAction['timeLeft'] -= TICK_TIME
degreesTurned = MAX_ROT_SPEED * TICK_TIME
# turn the right direction
if currentAction['amount'] < 0:
simulatedMe.currentRotation -= degreesTurned
else:
simulatedMe.currentRotation += degreesTurned
# keep the value in [-180, 180]
if simulatedMe.currentRotation < -180:
simulatedMe.currentRotation += 360
elif simulatedMe.currentRotation > 180:
simulatedMe.currentRotation -= 360
# if not, move the simulated robot forwards the lesser amount, then start the next action in the queue with the remaining time
else:
# calculate how far to turn
tickTimeLeft = TICK_TIME - currentAction['timeLeft']
degreesTurned = MAX_ROT_SPEED * currentAction['timeLeft']
# turn the right direction
if currentAction['amount'] < 0:
simulatedMe.currentRotation -= degreesTurned
else:
simulatedMe.currentRotation += degreesTurned
# keep the value in [-180, 180]
if simulatedMe.currentRotation < -180:
simulatedMe.currentRotation += 360
elif simulatedMe.currentRotation > 180:
simulatedMe.currentRotation -= 360
# report that we've finished executing this command
self.currentActionQueue.pop(0)
self.lastCommandFinished += 1
# start the next action if it's queued
self.tick(tickTimeLeft)
# if currently kicking
elif currentAction['action'] == SimulatorActions.kick:
# if it'll keep kicking for this whole tick, the only potential change is that the ball starts moving
if TICK_TIME < currentAction['timeLeft']:
currentAction['timeLeft'] -= TICK_TIME
#simulatedBall.get_kicked()
# if not, start the next action in the queue with the remaining time
else:
tickTimeLeft = TICK_TIME - currentAction['timeLeft']
# report that we've finished executing this command
self.currentActionQueue.pop(0)
self.lastCommandFinished += 1
# start the next action if it's queued
self.tick(tickTimeLeft)
# if currently grabbing
elif currentAction['action'] == SimulatorActions.grab:
# if it'll keep grabbing for this whole tick, the only potential change is that the ball stops moving
if TICK_TIME < currentAction['timeLeft']:
currentAction['timeLeft'] -= TICK_TIME
# if the ball is close enough stop the ball and toggle 'holding ball' state
#simulatedBall.get_grabbed(simulatedMe)
# if not, start the next action in the queue with the remaining time TODO
else:
self.grabbed = True
tickTimeLeft = TICK_TIME - currentAction['timeLeft']
# report that we've finished executing this command
self.currentActionQueue.pop(0)
self.lastCommandFinished += 1
# start the next action if it's queued
self.tick(tickTimeLeft)
# if currently ungrabbing
elif currentAction['action'] == SimulatorActions.ungrab:
# if it'll keep grabbing for this whole tick, nothing changes
if TICK_TIME < currentAction['timeLeft']:
currentAction['timeLeft'] -= TICK_TIME
# if not, start the next action in the queue with the remaining time
else:
self.grabbed = False
tickTimeLeft = TICK_TIME - currentAction['timeLeft']
# report that we've finished executing this command
self.currentActionQueue.pop(0)
self.lastCommandFinished += 1
# start the next action if it's queued
self.tick(tickTimeLeft)
def updatePositions():
"""Updates the system's belief of the state of the game based on the vision system"""
# get the info on the robots from the vision system
if api.getMyPosition() is not None:
mePosition = api.getMyPosition()
me.update(Point(mePosition[0]*X_RATIO,mePosition[1]*Y_RATIO))
else:
print "Can't find my position this tick :("
if api.getMyOrientation() is not None:
meOrientation = api.getMyOrientation()[1]
me.updateRotation(meOrientation)
else:
print "Can't find my orientation this tick :("
if api.getAllyPosition() is not None:
allyPosition = api.getAllyPosition()
ally.update(Point(allyPosition[0]*X_RATIO,allyPosition[1]*Y_RATIO))
else:
print "Can't find my friend's position this tick :("
if api.getAllyOrientation() is not None:
allyOrientation = api.getAllyOrientation()[1]
ally.updateRotation(allyOrientation)
else:
print "Can't find my friend's orientation this tick :("
if api.getEnemyPositions()[0] is not None:
enemy0Position = api.getEnemyPositions()[0]
enemies[0].update(Point(enemy0Position[0]*X_RATIO,enemy0Position[1]*Y_RATIO))
else:
print "Can't find enemy 0 this tick :("
if api.getEnemyOrientation()[0] is not None:
enemy0Orientation = api.getEnemyOrientation()[0][1]
enemies[0].updateRotation(enemy0Orientation)
else:
print "Can't find enemy 0's orientation this tick :("
if api.getEnemyPositions()[1] is not None:
enemy1Position = api.getEnemyPositions()[1]
enemies[1].update(Point(enemy1Position[0]*X_RATIO,enemy1Position[1]*Y_RATIO))
else:
print "Can't find enemy 1 this tick :("
if api.getEnemyOrientation()[1] is not None:
enemy1Orientation = api.getEnemyOrientation()[1][1]
enemies[1].updateRotation(enemy1Orientation)
else:
print "Can't find enemy 1's orientation this tick :("
if api.getBallCenter() is not None:
ballPosition = api.getBallCenter()
ball.update(Point(ballPosition[0]*X_RATIO,ballPosition[1]*Y_RATIO))
else:
print "Shit! Where's the ball gone"
try:#see who has ball posesion - needs work
if nearEnough(enemies[0].bearing(ball),enemies[0].currentRotation, near_enough_angle=45) and ball.distance(enemies[0]) < BALL_OWNERSHIP_DISTANCE:
ball.status = BallStatus.enemyA
elif nearEnough(enemies[1].bearing(ball),enemies[1].currentRotation, near_enough_angle=45) and ball.distance(enemies[1]) < BALL_OWNERSHIP_DISTANCE:
ball.status = BallStatus.enemyB
elif nearEnough(ally.bearing(ball),ally.currentRotation, near_enough_angle=45) and ball.distance(ally)< BALL_OWNERSHIP_DISTANCE:
ball.status = BallStatus.ally
elif nearEnough(me.bearing(ball), me.currentRotation, near_enough_angle=45) and ball.distance(me)< BALL_OWNERSHIP_DISTANCE and me.grabbed:
ball.status = BallStatus.me
# if we can't see it, assume it's the same
elif api.world['ball_center']==None:
pass
# if it's far enough from everything, it's free
else:
ball.status = BallStatus.free
except (TypeError, AttributeError):
print("Location of some objects unknown")