private void stepStartState(float dt) {
// start moving diagonally away from the corner
ramses.setHeading(-45.0f);
ramses.setPower(1.0f);
// after some time, switch to searching
if (duration > startTime) {
setState(State.SEARCH_BALL);
}
}
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 stepRelocateState(float dt) {
if (!stateReady) {
currentRelocateHeading = lastRelocateHeading + 90.0f % 360.0f;
lastRelocateHeading = currentRelocateHeading;
stateReady = true;
}
// drive ahead full power, this can be made much smarter to avoid walls etc
ramses.setYawRate(0.0f);
ramses.setHeading(currentRelocateHeading);
ramses.setPower(1.0f);
// after given time, go to requested next state
if (stateDuration > relocateTime) {
setState(nextState);
}
}