public ModifiedGoToObjectPFN(
double stopDistance,
boolean slowDownCloseToTarget,
double opponentPower,
double opponentInfluenceDistance,
double targetPower,
double alpha) {
this.stopDistance = 0;
setSlowDownCloseToTarget(slowDownCloseToTarget);
OPPONENT_POWER = opponentPower;
OPPONENT_INFLUENCE_DISTANCE = opponentInfluenceDistance;
TARGET_POWER = targetPower;
ALPHA = alpha;
setSlowDownCloseToTarget(slowDownCloseToTarget);
RobotConf conf = new RobotConf(ROBOT_TRACK_WIDTH, WHEEL_RADIUS);
// plann = new PFPlanning(conf, 0, 1, 12, 0.5); // No opponent avoidance
// Avoids opponent:
plann = new PFPlanning(conf, OPPONENT_POWER, OPPONENT_INFLUENCE_DISTANCE, TARGET_POWER, ALPHA);
// Add walls
double wallPower = 10;
double wallInfDist = -0.05;
RectObject leftWall =
new RectObject(new Point(0, Globals.PITCH_HEIGHT), new Point(0, 0), wallPower, wallInfDist);
RectObject rightWall =
new RectObject(
new Point(Globals.PITCH_WIDTH, Globals.PITCH_HEIGHT),
new Point(Globals.PITCH_WIDTH, 0),
wallPower,
wallInfDist);
RectObject topWall =
new RectObject(
new Point(0, Globals.PITCH_HEIGHT),
new Point(Globals.PITCH_WIDTH, Globals.PITCH_HEIGHT),
wallPower,
wallInfDist);
RectObject bottomWall =
new RectObject(new Point(0, 0), new Point(Globals.PITCH_WIDTH, 0), wallPower, wallInfDist);
plann.addObject(leftWall);
plann.addObject(rightWall);
plann.addObject(topWall);
plann.addObject(bottomWall);
}
@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);
}