private void stepFetchState(float dt) {
ramses.setPower(0.0f);
// quick brake
if (stateDuration < 0.2f) {
ramses.setYawRate(-1.0f);
return;
}
List<Camera.BallInfo> balls = ramses.getCamera().getVisibleBalls();
Camera.BallInfo target = null;
for (Camera.BallInfo ball : balls) {
if (ball.id == targetId) {
target = ball;
break;
}
}
if (target == null) {
// can't see the ball any more, search for new one
setState(State.SEARCH_BALL);
return;
}
// map the ball angle to yaw power
float angleDegrees = target.angle / (float) Math.PI * 180.0f;
float yawRate = Math.max(Math.min(angleDegrees / 60.0f, 1.0f), -1.0f);
// System.out.println("#" + target.id + " (" + target._getRealId() + ") angle: " + angleDegrees
// + "; yaw rate: " + yawRate);
ramses.setYawRate(yawRate);
// once the angle to ball is quite small, start moving towards it
if (Math.abs(angleDegrees) <= maxApproachAngle) {
// move slower when closer, also camera distance is further away than edge
float power = Math.max(Math.min((target.distance - 0.33f) / 1.0f, 1.0f), 0.2f);
// this one is fun to watch, like a drunkard!
// ramses.setHeading(-angleDegrees);
ramses.setHeading(0.0f);
ramses.setPower(power);
if (ramses.getDribbler().hasBall()) {
setState(State.SEARCH_GOAL);
return;
}
}
}
private void stepGuessedAngleUpdater(float dt) {
List<Camera.GoalInfo> goals = ramses.getCamera().getVisibleGoals();
Simulation.Side oppositeSide =
ramses.getSide() == Simulation.Side.BLUE ? Simulation.Side.YELLOW : Simulation.Side.BLUE;
for (Camera.GoalInfo goal : goals) {
if (goal.side == oppositeSide) {
ramses.setGuessedAngleToGoal(goal.angle / (float) Math.PI * 180.0f);
} else {
ramses.setGuessedAngleToGoal(goal.angle / (float) Math.PI * 180.0f - 180.0f);
}
}
}
private void stepSearchGoal(float dt) {
if (!stateReady) {
targetGoalSightDuration = -1.0f;
searchYawDir = 1.0f;
stateReady = true;
}
if (!ramses.getDribbler().hasBall()) {
setState(State.SEARCH_BALL);
return;
}
ramses.setPower(0.0f);
List<Camera.GoalInfo> goals = ramses.getCamera().getVisibleGoals();
Simulation.Side oppositeSide =
ramses.getSide() == Simulation.Side.BLUE ? Simulation.Side.YELLOW : Simulation.Side.BLUE;
Camera.GoalInfo oppositeGoal = null;
for (Camera.GoalInfo goal : goals) {
if (goal.side == oppositeSide) {
oppositeGoal = goal;
if (targetGoalSightDuration == -1.0f) {
targetGoalSightDuration = 0.0f;
float guessedAngle = ramses.getGuessedAngleToGoal();
if (guessedAngle > 0.0f && guessedAngle < 180.0f) {
searchYawDir = 1.0f;
} else {
searchYawDir = -1.0f;
}
} else {
targetGoalSightDuration += dt;
}
break;
}
}
if (oppositeGoal != null) {
if (targetGoalSightDuration < 0.2f) {
// quick brake when first sighting the goal
float guessedAngle = ramses.getGuessedAngleToGoal();
if (guessedAngle > 0.0f) {
ramses.setYawRate(-1.0f);
} else {
ramses.setYawRate(1.0f);
}
} else {
// we can see the goal, turn to it
float angleDegrees = oppositeGoal.angle / (float) Math.PI * 180.0f;
// we know the angle right know, set it to improve guess later
ramses.setGuessedAngleToGoal(angleDegrees);
// the required accuracy depends on how far away the goal is - the closer
// we are, the more we can miss the center
float requiredAccuracy = Math.max(10.0f / (oppositeGoal.distance * 5.0f), 2.0f);
if (angleDegrees <= requiredAccuracy) {
// goal is centered, shoot!
ramses.kick();
targetId = -1;
realTargetId = -1;
} else {
// goal is not centered enough, correct
float yawRate = Math.max(Math.min(angleDegrees / 30.0f, 1.0f), -1.0f);
ramses.setYawRate(yawRate);
}
}
} else {
ramses.setYawRate(0.25f * searchYawDir);
targetGoalSightDuration = -1.0f;
}
}
private void stepSearchBallState(float dt) {
// spin around at current position
ramses.setPower(0.0f);
if (!stateReady) {
// reset closest distance in first frame of state
closestBallDistance = Float.MAX_VALUE;
targetId = -1;
realTargetId = -1;
if (lastBallSearchDir == 1.0f) {
currentBallSearchDir = -1.0f;
} else {
currentBallSearchDir = 1.0f;
}
lastBallSearchDir = currentBallSearchDir;
stateReady = true;
}
if (stateDuration <= spinSearchTime) {
// spin and find the closes ball distance
List<Camera.BallInfo> balls = ramses.getCamera().getVisibleBalls();
if (balls.size() == 0) {
// no balls visible, spin faster, might not be a good idea with a real
// camera though
ramses.setYawRate(0.5f * currentBallSearchDir);
} else {
// we can see something, slow dont not to pass quikcly
ramses.setYawRate(0.25f * currentBallSearchDir);
}
for (Camera.BallInfo ball : balls) {
if (ball.distance < closestBallDistance) {
closestBallDistance = ball.distance;
// System.out.println("New closest: #" + ball.id + " at " + ball.distance + "m");
}
}
// pick the closest of close enough balls
if (closestBallDistance <= closeEnoughThreshold) {
for (Camera.BallInfo ball : balls) {
if (ball.distance == closestBallDistance) {
// found a ball close enough, skip searching closest
targetId = ball.id;
realTargetId = ball._getRealId();
setState(State.FETCH);
}
}
}
} else if (stateDuration < spinSearchTime + refindTime) {
// find that closest ball again
List<Camera.BallInfo> balls = ramses.getCamera().getVisibleBalls();
if (balls.size() == 0) {
// no balls visible, spin faster, might not be a good idea with a real
// camera though
ramses.setYawRate(0.25f * currentBallSearchDir);
} else {
// we can see something, slow dont not to pass quikcly
ramses.setYawRate(0.1f * currentBallSearchDir);
}
for (Camera.BallInfo ball : balls) {
if (Math.abs(ball.distance - closestBallDistance) <= distanceCompareThreshold) {
// System.out.println("Found #" + ball.id + " (" + ball._getRealId() + ") at " +
// ball.distance);
// found a ball at similar distance, set as target
targetId = ball.id;
realTargetId = ball._getRealId();
setState(State.FETCH);
return;
}
}
} else {
// didn't find a ball, move around a bit
setState(State.RELOCATE, State.SEARCH_BALL);
}
}