@Override
public void onStep(Controller controller, Snapshot snapshot) throws ConfusedException {
FieldObject target = getTarget(snapshot);
if ((snapshot == null) || (snapshot.getBalle().getPosition() == null) || (target == null))
return;
Orientation usToTar, midToTar, tarToGoal;
usToTar = target.getPosition().sub(snapshot.getBalle().getPosition()).getOrientation();
midToTar =
snapshot.getPitch().getPosition().sub(snapshot.getBalle().getPosition()).getOrientation();
tarToGoal =
snapshot.getOpponentsGoal().getPosition().sub(target.getPosition()).getOrientation();
if (midToTar.sub(midToTar).abs().degrees() > 90)
executorStrategy.updateTarget(target, midToTar);
else executorStrategy.updateTarget(target, usToTar);
// Draw the target
addDrawables(executorStrategy.getDrawables());
if (target.getPosition() != null) addDrawable(new Dot(target.getPosition(), getTargetColor()));
// If it says it is not finished, tell it to do something for a step.
if (!executorStrategy.isFinished(snapshot)) {
executorStrategy.step(controller, snapshot);
} else {
// Tell the strategy to stop doing whatever it was doing
executorStrategy.stop(controller);
}
}
public MovementDirection getMovementDirection(Snapshot snapshot) {
Robot opponent = snapshot.getOpponent();
Robot ourRobot = snapshot.getBalle();
Goal ourGoal = snapshot.getOwnGoal();
Ball ball = snapshot.getBall();
if ((opponent.getPosition() == null) || (ourRobot.getPosition() == null))
return MovementDirection.NONE;
Coord intersectionPoint = null;
if ((ball.getPosition() != null) && (!ball.getPosition().isEstimated()))
intersectionPoint =
opponent.getBallKickLine(ball).getIntersect(ourRobot.getFacingLineBothWays());
if (intersectionPoint == null)
intersectionPoint = opponent.getFacingLine().getIntersect(ourRobot.getFacingLineBothWays());
if (intersectionPoint == null) return MovementDirection.NONE;
double yCoord = intersectionPoint.getY();
yCoord = Math.max(yCoord, ourGoal.getMinY() + Globals.ROBOT_LENGTH / 3);
yCoord = Math.min(yCoord, ourGoal.getMaxY() - Globals.ROBOT_LENGTH / 3);
Coord targetPoint = new Coord(ourRobot.getPosition().getX(), yCoord);
addDrawable(new Dot(targetPoint, Color.WHITE));
boolean isUpward;
Orientation ourOrientation = ourRobot.getOrientation();
if ((ourOrientation.radians() >= 0) && (ourOrientation.radians() < Math.PI)) isUpward = true;
else isUpward = false;
// If we are already blocking the point stop
if (ourRobot.containsCoord(targetPoint)) return MovementDirection.NONE;
double diff = (targetPoint.getY() - ourRobot.getPosition().getY());
if (diff > 0)
if (isUpward) return MovementDirection.FORWARD;
else return MovementDirection.BACKWARD;
else if (isUpward) return MovementDirection.BACKWARD;
else return MovementDirection.FORWARD;
}
@Override
public void onStep(Controller controller, Snapshot snapshot) throws ConfusedException {
if (snapshot.getBalle().getPosition() == null) return;
// Make sure to reset the speeds if we haven't been dribbling for a
// while
long currentTime = System.currentTimeMillis();
boolean facingOwnGoalSide = snapshot.getBalle().isFacingGoalHalf(snapshot.getOwnGoal());
lastDribbled = currentTime;
boolean facingGoal =
snapshot.getBalle().getFacingLine().intersects(snapshot.getOpponentsGoal().getGoalLine());
if (snapshot.getBall().getPosition() != null)
facingGoal =
facingGoal
|| snapshot
.getBalle()
.getBallKickLine(snapshot.getBall())
.intersects(snapshot.getOpponentsGoal().getGoalLine());
if (currentSpeed <= 560) {
currentSpeed += 20;
}
if (turnSpeed <= 150) {
turnSpeed += 5;
}
double distanceToBall =
snapshot.getBalle().getFrontSide().midpoint().dist(snapshot.getBall().getPosition());
if (snapshot.getBall().getPosition().isEstimated()) distanceToBall = 0;
boolean aboutToLoseBall = distanceToBall >= ABOUT_TO_LOSE_BALL_THRESHOLD;
Color c = Color.BLACK;
if (aboutToLoseBall) c = Color.PINK;
Coord ourPos = snapshot.getBalle().getPosition();
addDrawable(
new Label(
String.format("%.5f", distanceToBall), new Coord(ourPos.getX(), ourPos.getY()), c));
int turnSpeedToUse = turnSpeed;
boolean isLeftGoal = snapshot.getOpponentsGoal().isLeftGoal();
double angle = snapshot.getBalle().getOrientation().radians();
double threshold = Math.toRadians(5);
boolean nearWall = snapshot.getBall().isNearWall(snapshot.getPitch());
boolean wereNearWall = snapshot.getBalle().isNearWall(snapshot.getPitch(), SPINNING_DISTANCE);
boolean closeToGoal =
snapshot.getOpponentsGoal().getGoalLine().dist(snapshot.getBalle().getPosition())
< SPINNING_DISTANCE;
// Actually it might be helpful to turn when we're in this situation
// close to our own goal
closeToGoal =
closeToGoal
|| snapshot.getOwnGoal().getGoalLine().dist(snapshot.getBalle().getPosition())
< SPINNING_DISTANCE;
// Turn twice as fast near walls
if (nearWall) turnSpeedToUse *= 2;
if ((!closeToGoal) && (nearWall) && wereNearWall) {
Coord goalVector = snapshot.getOwnGoal().getGoalLine().midpoint().sub(ourPos);
Orientation angleTowardsGoal = goalVector.orientation();
// Always turn opposite from own goal
boolean shouldTurnRight = !angleTowardsGoal.isFacingRight(0);
// If we're facing wall
if ((Math.abs(angle) <= FACING_WALL_THRESHOLD)
|| (Math.abs(angle - Math.PI / 2) <= FACING_WALL_THRESHOLD)
|| (Math.abs(angle - Math.PI) <= FACING_WALL_THRESHOLD)
|| (Math.abs(angle - 3 * Math.PI / 2) <= FACING_WALL_THRESHOLD)) {
LOG.info("Spinning!!!");
// If We are facing the bottom wall we should flip the spinning
// directions
if (Math.abs(angle - 3 * Math.PI / 2) <= FACING_WALL_THRESHOLD)
shouldTurnRight = !shouldTurnRight;
else if ((Math.abs(angle) <= FACING_WALL_THRESHOLD)
|| (Math.abs(angle - Math.PI) <= FACING_WALL_THRESHOLD)) {
// If we're facing one of the walls with goals
boolean facingLeftWall = (Math.abs(angle) <= FACING_WALL_THRESHOLD);
if (facingLeftWall) {
shouldTurnRight =
snapshot.getOpponentsGoal().getGoalLine().midpoint().getY() > ourPos.getY();
// Turn away from own goal
if (snapshot.getOwnGoal().isLeftGoal()) shouldTurnRight = !shouldTurnRight;
} else {
shouldTurnRight =
snapshot.getOpponentsGoal().getGoalLine().midpoint().getY() < ourPos.getY();
// Turn away from own goal
if (snapshot.getOwnGoal().isRightGoal()) shouldTurnRight = !shouldTurnRight;
}
}
if (shouldTurnRight) spinRight(snapshot, controller, Globals.MAXIMUM_MOTOR_SPEED);
else spinLeft(snapshot, controller, Globals.MAXIMUM_MOTOR_SPEED);
return;
}
}
if (isLeftGoal) {
if (facingGoal) {
controller.setWheelSpeeds(Globals.MAXIMUM_MOTOR_SPEED, Globals.MAXIMUM_MOTOR_SPEED);
} else if ((!facingGoal) && (angle < Math.PI - threshold)) {
controller.setWheelSpeeds(currentSpeed, currentSpeed + turnSpeedToUse);
} else if ((!facingGoal) && (angle > Math.PI + threshold)) {
controller.setWheelSpeeds(currentSpeed + turnSpeedToUse, currentSpeed);
} else {
controller.setWheelSpeeds(currentSpeed, currentSpeed);
}
} else {
if (facingGoal) {
controller.setWheelSpeeds(Globals.MAXIMUM_MOTOR_SPEED, Globals.MAXIMUM_MOTOR_SPEED);
} else if ((!facingGoal) && (angle > threshold) && (angle < Math.PI)) {
controller.setWheelSpeeds(currentSpeed + turnSpeedToUse, currentSpeed);
} else if ((!facingGoal) && (angle < (2 * Math.PI) - threshold) && (angle > Math.PI)) {
controller.setWheelSpeeds(currentSpeed, currentSpeed + turnSpeedToUse);
} else {
controller.setWheelSpeeds(currentSpeed, currentSpeed);
}
}
}
