Exemplo n.º 1
0
  public static MovementDecision toMovementDecision(
      LXXRobotState robot, double targetHeading, MovementDirection movementDirection) {
    final double robotHeading =
        movementDirection == MovementDirection.FORWARD
            ? robot.getHeadingRadians()
            : Utils.normalAbsoluteAngle(robot.getHeadingRadians() + LXXConstants.RADIANS_180);
    final double neededTurnRateRadians = Utils.normalRelativeAngle(targetHeading - robotHeading);
    double turnRateRadians = neededTurnRateRadians;
    final double speed = robot.getVelocityModule();
    final double acceleratedSpeed = min(speed + 1, Rules.MAX_VELOCITY);
    if (abs(turnRateRadians) > Rules.getTurnRateRadians(acceleratedSpeed)) {
      turnRateRadians = Rules.getTurnRateRadians(acceleratedSpeed) * signum(turnRateRadians);
    }
    final double acceleration = getAcceleration(robot, turnRateRadians, robotHeading);
    turnRateRadians =
        min(
                abs(neededTurnRateRadians),
                abs(
                    Rules.getTurnRateRadians(
                        LXXUtils.limit(0, speed + acceleration, Rules.MAX_VELOCITY))))
            * signum(turnRateRadians);

    return new MovementDecision(acceleration, turnRateRadians, movementDirection);
  }
Exemplo n.º 2
0
  private static double getAcceleration(
      LXXRobotState robot, double turnRateRadians, double robotHeading) {
    final double speed = min(robot.getVelocityModule(), Rules.MAX_VELOCITY);
    final double acceleratedSpeed = min(speed + 1, Rules.MAX_VELOCITY);
    final double deceleratedSpeed1 = max(speed - 1, 0);
    final double newHeading = Utils.normalAbsoluteAngle(robotHeading + turnRateRadians);
    final int acceleration;
    if (robot
        .getBattleField()
        .contains(robot.project(newHeading, LXXUtils.getStopDistance(acceleratedSpeed) + 12))) {
      acceleration = 1;
    } else if (robot
        .getBattleField()
        .contains(robot.project(newHeading, LXXUtils.getStopDistance(speed) + 12))) {
      acceleration = 0;
    } else if (robot
        .getBattleField()
        .contains(robot.project(newHeading, LXXUtils.getStopDistance(deceleratedSpeed1) + 12))) {
      acceleration = -1;
    } else {
      acceleration = -2;
    }

    if (robot.getVelocityModule() + acceleration > Rules.MAX_VELOCITY) {
      return Rules.MAX_VELOCITY - robot.getVelocityModule();
    }
    if (robot.getVelocityModule() + acceleration < 0) {
      return -robot.getVelocityModule();
    }

    return acceleration;
  }