def play(self):
self.updateLocations()
self.commandRoboclaws()
#print (self.robotLocation.x, self.robotLocation.y)
#self.setSpeed()
#check if robot is ready to rush goal
if self.state == State.check:
self.state = State.getBehindBall
if self.robotLocation.x > pixelToMeter(345):
self.state = State.returnToPlay
elif (MotionSkills.isPointInFrontOfRobot(
self.robotLocation, self.ball.point, 0.5,
0.04 + abs(MAX_SPEED /
4))): #This offset compensates for the momentum
self.state = State.rushGoal # rush goal
self.stopRushingGoalTime = getTime() + int(
2 * DIS_BEHIND_BALL / MAX_SPEED * 100)
if self.state == State.rushGoal:
#self.speed = RUSH_SPEED
#self.go_to_point(HOME_GOAL.x, HOME_GOAL.y, HOME_GOAL)
self.go_direction(HOME_GOAL)
if getTime() >= self.stopRushingGoalTime:
kick()
self.state = State.check
if self.state == State.returnToPlay:
self.go_to_point(CENTER.x, CENTER.y, HOME_GOAL)
if abs(self.robotLocation.x) < .2 and abs(
self.robotLocation.y) < .2:
self.state = State.check
#check if ball is behind robot
if self.state == State.getBehindBall:
#robot in front of ball
if MotionSkills.isBallBehindRobot(self.robotLocation,
self.ball.point):
# This gets a point beside the ball perpendicular to the line of the ball and the goal
#point = getPointBesideBall(self.robotLocation, self.ball.point, DIS_BEHIND_BALL)
point = Point(self.ball.point.x)
# if robot above ball
if self.ball.point.y < self.robotLocation.y:
point.y = self.ball.point.y + DIS_BEHIND_BALL
else:
point.y = self.ball.point.y - DIS_BEHIND_BALL
self.go_direction(point)
#Move to the side of the ball
#self.go_to_point(point.x,point.y)
#robot behind ball
else:
behindTheBallPoint = MotionSkills.getPointBehindBall(self.ball)
self.go_direction(behindTheBallPoint)
self.state = State.check
def play(self):
self.updateLocations()
self.commandRoboclaws()
#print (self.robotLocation.x, self.robotLocation.y)
#self.setSpeed()
#check if robot is ready to rush goal
if self.state == State.check:
self.state = State.getBehindBall
if self.robotLocation.x > pixelToMeter(345):
self.state = State.returnToPlay
elif (MotionSkills.isPointInFrontOfRobot(self.robotLocation,self.ball.point, 0.5, 0.04 + abs(MAX_SPEED/4))): #This offset compensates for the momentum
self.state = State.rushGoal# rush goal
self.stopRushingGoalTime = getTime() + int(2 * DIS_BEHIND_BALL/MAX_SPEED*100)
if self.state == State.rushGoal:
#self.speed = RUSH_SPEED
#self.go_to_point(HOME_GOAL.x, HOME_GOAL.y, HOME_GOAL)
self.go_direction(HOME_GOAL)
if getTime() >= self.stopRushingGoalTime:
kick()
self.state = State.check
if self.state == State.returnToPlay:
self.go_to_point(CENTER.x, CENTER.y, HOME_GOAL)
if abs(self.robotLocation.x) < .2 and abs(self.robotLocation.y) < .2:
self.state = State.check
#check if ball is behind robot
if self.state == State.getBehindBall:
#robot in front of ball
if MotionSkills.isBallBehindRobot(self.robotLocation, self.ball.point):
# This gets a point beside the ball perpendicular to the line of the ball and the goal
#point = getPointBesideBall(self.robotLocation, self.ball.point, DIS_BEHIND_BALL)
point = Point(self.ball.point.x)
# if robot above ball
if self.ball.point.y < self.robotLocation.y:
point.y = self.ball.point.y + DIS_BEHIND_BALL
else:
point.y = self.ball.point.y - DIS_BEHIND_BALL
self.go_direction(point)
#Move to the side of the ball
#self.go_to_point(point.x,point.y)
#robot behind ball
else:
behindTheBallPoint = MotionSkills.getPointBehindBall(self.ball)
self.go_direction(behindTheBallPoint)
self.state = State.check
def test(self):
global HOME_GOAL
self.updateLocations()
HOME_GOAL = Point(HOME_GOAL.x, 0)
# Kick the ball off of the sides when it is too close to the side
if self.ball.point.y < (MARGIN - HEIGHT_FIELD_METER / 2):
HOME_GOAL = Point(HOME_GOAL.x, -HEIGHT_FIELD_METER)
if self.ball.point.y > (HEIGHT_FIELD_METER / 2 - MARGIN):
HOME_GOAL = Point(HOME_GOAL.x, HEIGHT_FIELD_METER)
# Make sure the robot is behind the ball and facing the goal
if self.testState == TestState.check:
self.testState = TestState.getBehindBall
angleBallGoal = MotionSkills.angleBetweenPoints(
self.ball.point, HOME_GOAL)
deltaAngle = MotionSkills.deltaBetweenAngles(
self.robotLocation.theta, angleBallGoal)
if MotionSkills.isPointInFrontOfRobot(
self.robotLocation,
self.ball.point) and abs(deltaAngle) < .12:
self.testState = TestState.rushGoal
self.stopRushingGoalTime = getTime() + 45
# Get behind the ball. Move to the side first is necessary
if self.testState == TestState.getBehindBall:
if (self.ball.point.x + .05) < (self.robotLocation.x):
point = Point(self.ball.point.x)
if self.ball.point.y < self.robotLocation.y:
point.y = self.ball.point.y + DIS_BEHIND_BALL
else:
point.y = self.ball.point.y - DIS_BEHIND_BALL
#Move to the side of the ball
self.go_to_point(point.x, point.y)
#Go to behind ball
else:
behindTheBallPoint = MotionSkills.getPointBehindBall(
self.ball, HOME_GOAL)
self.go_to_point(behindTheBallPoint.x, behindTheBallPoint.y)
self.testState = TestState.check
# Rush the goal and kick for the specified time
if self.testState == TestState.rushGoal:
self.go_to_point(HOME_GOAL.x, HOME_GOAL.y, HOME_GOAL)
if self.distanceToBall < .05:
kick()
if self.stopRushingGoalTime <= getTime():
self.testState = TestState.check
def test(self):
global HOME_GOAL
self.updateLocations()
HOME_GOAL = Point(HOME_GOAL.x,0)
# Kick the ball off of the sides when it is too close to the side
if self.ball.point.y < (MARGIN - HEIGHT_FIELD_METER/2):
HOME_GOAL = Point(HOME_GOAL.x, -HEIGHT_FIELD_METER)
if self.ball.point.y > (HEIGHT_FIELD_METER/2 - MARGIN):
HOME_GOAL = Point(HOME_GOAL.x, HEIGHT_FIELD_METER)
# Make sure the robot is behind the ball and facing the goal
if self.testState == TestState.check:
self.testState = TestState.getBehindBall
angleBallGoal = MotionSkills.angleBetweenPoints(self.ball.point,HOME_GOAL)
deltaAngle = MotionSkills.deltaBetweenAngles(self.robotLocation.theta,angleBallGoal)
if MotionSkills.isPointInFrontOfRobot(self.robotLocation,self.ball.point) and abs(deltaAngle) < .12:
self.testState = TestState.rushGoal
self.stopRushingGoalTime = getTime() + 45
# Get behind the ball. Move to the side first is necessary
if self.testState == TestState.getBehindBall:
if (self.ball.point.x + .05) < (self.robotLocation.x):
point = Point(self.ball.point.x)
if self.ball.point.y < self.robotLocation.y:
point.y = self.ball.point.y + DIS_BEHIND_BALL
else:
point.y = self.ball.point.y - DIS_BEHIND_BALL
#Move to the side of the ball
self.go_to_point(point.x,point.y)
#Go to behind ball
else:
behindTheBallPoint = MotionSkills.getPointBehindBall(self.ball,HOME_GOAL)
self.go_to_point(behindTheBallPoint.x,behindTheBallPoint.y)
self.testState = TestState.check
# Rush the goal and kick for the specified time
if self.testState == TestState.rushGoal:
self.go_to_point(HOME_GOAL.x,HOME_GOAL.y,HOME_GOAL)
if self.distanceToBall < .05:
kick()
if self.stopRushingGoalTime <= getTime():
self.testState = TestState.check
