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