@Override
public void onStep(Controller controller, Snapshot snapshot) throws ConfusedException {
if (finished || !isStillInPenaltyDefence(snapshot)) {
super.onStep(controller, snapshot);
return;
}
if (snapshot.getBalle().getPosition() == null) {
return;
}
if ((firstSnapshot == null) && (snapshot.getBall().getPosition() != null)) {
firstSnapshot = snapshot;
}
MovementDirection movementDirection = getMovementDirection(snapshot);
if (movementDirection == MovementDirection.FORWARD) controller.setWheelSpeeds(SPEED, SPEED);
else if (movementDirection == MovementDirection.BACKWARD)
controller.setWheelSpeeds(-SPEED, -SPEED);
else controller.stop();
}
@Override
public void step(Controller controller, Snapshot snapshot) {
drawables.clear();
if (!isPossible(snapshot)) return;
Pos opponent;
if ((snapshot.getOpponent().getOrientation() == null)
|| (snapshot.getOpponent().getPosition() == null)) opponent = null;
else
opponent =
new Pos(
new Point(
snapshot.getOpponent().getPosition().getX(),
snapshot.getOpponent().getPosition().getY()),
snapshot.getOpponent().getOrientation().radians());
// Our pos
Pos initPos =
new Pos(
new Point(
snapshot.getBalle().getPosition().getX(), snapshot.getBalle().getPosition().getY()),
snapshot.getBalle().getOrientation().radians());
// Target pos
Point targetLoc = null;
Point targetLoc2 = new Point(target.getPosition().getX(), target.getPosition().getY());
System.out.println(target.getPosition().getY());
if (target.getPosition().getY() >= 0.6) {
targetLoc = new Point(target.getPosition().getX(), target.getPosition().getY() - 0.2);
} else {
targetLoc = new Point(target.getPosition().getX(), target.getPosition().getY() + 0.2);
}
if (snapshot.getBalle().getPosition().dist(new Coord(targetLoc.getX(), targetLoc.getY()))
< 0.1) {
targetLoc = targetLoc2;
}
VelocityVec res = plann.update(initPos, opponent, targetLoc);
if (!shouldSlowDownCloseToTarget()) {
Vector newRes = res.mult(4);
res = new VelocityVec(newRes.getX(), newRes.getY());
}
LOG.trace(
"UNSCALED Left speed: "
+ Math.toDegrees(res.getLeft())
+ " right speed: "
+ Math.toDegrees(res.getRight()));
double left, right;
res = res.scale();
left = Math.toDegrees(res.getLeft());
right = Math.toDegrees(res.getRight());
drawables.add(
new Label(
String.format("(%d, %d)", (int) left, (int) right),
new Coord(1, Globals.PITCH_HEIGHT + 0.3),
Color.BLACK));
controller.setWheelSpeeds((int) left, (int) right);
}
@Override
public void stop(Controller controller) {
controller.stop();
}
@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);
}
}
}
public void spinRight(Snapshot snapshot, Controller controller, int speed) {
controller.setWheelSpeeds(speed, -speed);
addDrawable(new Label("--->", snapshot.getBalle().getPosition(), Color.CYAN));
}
