Java Alliance.Red方法代码示例

import edu.wpi.first.wpilibj.DriverStation.Alliance; //导入方法依赖的package包/类
protected void initialize() {
  logger.info("Initialize");

  startAngle = Robot.driveTrain.getYaw();
  
  if (DriverStation.getInstance().getAlliance() == Alliance.Red) {
    targetAngle = DriveTrain.fixDegrees(startAngle + 120);
    clockwise = true;
  } else {
    targetAngle = DriveTrain.fixDegrees(startAngle - 120);
    clockwise = false;
  }
}
 

import edu.wpi.first.wpilibj.DriverStation.Alliance; //导入方法依赖的package包/类
/**
 * Gets the current alliance FieldMap.
 * 
 * @return Either red map or blue map
 */
public static FieldMap getAllianceMap() {
  Alliance alliance = DriverStation.getInstance().getAlliance();
  if (alliance == Alliance.Blue) {
    return getBlue();
  } else if (alliance == Alliance.Red) {
    return getRed();
  } else {
    logger.error("Invalid alliance reported by DS!");
    return getBlue();
  }
}
 

import edu.wpi.first.wpilibj.DriverStation.Alliance; //导入方法依赖的package包/类
/**
 * Generates navigation to the boiler from the far side gear lift.
 * @param currentPosition The current robot position
 */
public static DrivePathCommand navigateFeederSideLiftToBoiler(RobotPosition currentPosition) {
  logger.info(String.format("Calculating path, start=%s", currentPosition));
  FieldMap map = getAllianceMap();

  FieldPosition boiler = map.boiler;
  Alliance alliance = DriverStation.getInstance().getAlliance();
  
  
  FieldPosition spline0 = currentPosition.add(alliance == Alliance.Red ? 1 : -1, 0);
  final double clearX = FieldPosition.CLEAR_DIVIDERS_TO_CENTER;
  FieldPosition clearOfDividersPosition =
      new FieldPosition(alliance == Alliance.Red ? -clearX : clearX, currentPosition.y);

  double distanceToBoiler = clearOfDividersPosition.distanceTo(boiler);

  List<FieldPosition> controlPoints = new LinkedList<FieldPosition>();
  controlPoints.add(spline0);
  controlPoints.add(currentPosition);
  controlPoints.add(clearOfDividersPosition);
  
  double ratio = RobotMap.SHOOTING_DISTANCE / distanceToBoiler;
  
  controlPoints.add(clearOfDividersPosition.multiply(1 - ratio).add(boiler.multiply(ratio)));
  
  controlPoints.add(boiler);
  List<FieldPosition> splinePoints = generateCatmullRomSpline(controlPoints);
  DrivePathCommand drivePathCommand = new DrivePathCommand(splinePoints);
  return drivePathCommand;
}
 

import edu.wpi.first.wpilibj.DriverStation.Alliance; //导入方法依赖的package包/类
/**
 * Generates navigation to the boiler from a starting position.
 * @param startingSide The robot starting configuration
 */
public static DrivePathCommand navigateStartToBoiler(FieldSide startingSide) {
  logger.info(String.format("Calculating path, start=%s", startingSide.toString()));
  FieldMap map = getAllianceMap();

  final Alliance alliance = DriverStation.getInstance().getAlliance();
  
  final FieldPosition startingPosition = map.startingPositions[startingSide.id];
  
  List<FieldPosition> controlPoints = new LinkedList<FieldPosition>();
  
  FieldPosition spline0 = startingPosition.add(alliance == Alliance.Red ? -12 : 12, 0);
  controlPoints.add(spline0);
  
  controlPoints.add(startingPosition);
  
  FieldPosition initialForward = startingPosition.add(alliance == Alliance.Red ? 24 : -24, 0);
  controlPoints.add(initialForward);

  if (startingSide != FieldSide.BOILER) {
    FieldPosition closeToAllianceWallPoint =
        new FieldPosition(alliance == Alliance.Red ? -297.545 : 297.545, -66);
    controlPoints.add(closeToAllianceWallPoint);
  }
  
  FieldPosition finalPoint =
      new FieldPosition(alliance == Alliance.Red ? -225.977 : 225.977, -94.018);
  controlPoints.add(finalPoint);
 
  controlPoints.add(new FieldPosition(alliance == Alliance.Red ? -200 : 200, -94.018));
  List<FieldPosition> splinePoints = generateCatmullRomSpline(controlPoints);
  DrivePathCommand drivePathCommand = new DrivePathCommand(splinePoints);
  return drivePathCommand;
}
 

import edu.wpi.first.wpilibj.DriverStation.Alliance; //导入方法依赖的package包/类
/**
 * Generates navigation from a starting position to the hopper.
 * @param startingPositionId The starting position ID
 * @return A DriveToPathCommand that drives the generated path
 */
public static DrivePathCommand navigateStartToHopper(int startingPositionId) { 
  FieldMap map = getAllianceMap();
  
  final Alliance alliance = DriverStation.getInstance().getAlliance();

  final FieldPosition hopperPosition =
      alliance == Alliance.Red ? red.hopperBoilerRed : blue.hopperBoilerBlue;

  if (startingPositionId != 0) {
    startingPositionId = 0;
    logger.error("New starting position: " + startingPositionId);
  }
  
  final List<FieldPosition> controlPoints = new LinkedList<FieldPosition>();

  FieldPosition startingPosition = map.startingPositions[startingPositionId];
  logger.debug(String.format("Starting position %s", startingPosition));
  
  // add a point behind us so the C-R spline generates correctly
  controlPoints.add(startingPosition.add(alliance == Alliance.Red ? -12 : 12, 0));

  // drive forward 2 feet
  FieldPosition initialForwardPosition =
      startingPosition.add(alliance == Alliance.Red ? 24 : -24, 0);
  logger.debug(String.format("2ft forward position %s", initialForwardPosition));
  
  controlPoints.add(initialForwardPosition);

  double initialDriveToX;
  double initialDriveToY;
  
  initialDriveToX = alliance == Alliance.Red
      ? hopperPosition.getX() - HOPPER_TRIGGER_WIDTH / 2 - RobotMap.ROBOT_WIDTH / 2 + 2.0
      : hopperPosition.getX() + HOPPER_TRIGGER_WIDTH / 2 + RobotMap.ROBOT_WIDTH / 2 - 2.0;
  initialDriveToY = hopperPosition.getY()
      - (RobotMap.ROBOT_LENGTH - RobotMap.ROBOT_SHOOTER_TO_TURN_CENTER_DIST);
  
  double splinePointX = initialDriveToX;
  double splinePointY = initialDriveToY - 24.0;

  FieldPosition initialDriveToPosition = new FieldPosition(initialDriveToX, initialDriveToY);
  logger.debug(String.format("Drive to position %s", initialDriveToPosition));
  controlPoints.add(initialDriveToPosition);
  
  FieldPosition splinePoint = new FieldPosition(splinePointX, splinePointY);
  logger.debug(String.format("Spline point %s", splinePoint));
  controlPoints.add(splinePoint);
  
  logger.info(controlPoints.toString());
  List<FieldPosition> splinePoints = generateCatmullRomSpline(controlPoints);
  logger.info(splinePoints.toString());
  
  DrivePathCommand drivePathCommand = new DrivePathCommand(splinePoints);
  return drivePathCommand;
}