Java 类edu.wpi.first.wpilibj.DriverStation.Alliance 实例源码

public void disabledPeriodic() {
    RobotMap.lightRing.set(Relay.Value.kOff);
    Scheduler.getInstance().run();

    recordedID = (String) (oi.index.getSelected());
    recordedAuton = SmartDashboard.getBoolean("Use Recorded Autonomous");

    Aimer.toPositionMode();
    RobotMap.winchMotor.setEncPosition(0);
    RobotMap.winchMotor.setPosition(0);
    RobotMap.winchMotor.set(0);
    DashboardOutput.putPeriodicData();

    isBlue = (DriverStation.getInstance().getAlliance() == Alliance.Blue);

    sendStateToLights(false, false);
}

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;
  }
}

/**
 * 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();
  }
}

/**
 * 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;
}

/**
 * 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;
}

private static byte [] createGamePacket() {
    DriverStation ds = DriverStation.getInstance();
    float matchTime = (float)ds.getMatchTime();
    float batteryVoltage = (float)ds.getBatteryVoltage();
    byte dsNumber = (byte)ds.getLocation();
    int teamNumber = ds.getTeamNumber();
    byte [] matchTimeRaw = intToByteArray(Float.floatToIntBits(matchTime));
    byte [] batteryVoltageRaw = intToByteArray(Float.floatToIntBits(batteryVoltage));
    byte [] teamNumberRaw = intToByteArray(teamNumber);
    byte miscData = 0;
    final byte [] ref = {(byte)0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01};
    miscData ^= (ds.getAlliance().value==Alliance.kBlue_val)?ref[0]:0; //(Blue, Red)(True, False)
    miscData ^= !ds.isEnabled() ? ref[1] : 0; // (Disabled, Enabled)
    miscData ^= ds.isAutonomous()? ref[2] : 0; // (Autonomous, )
    miscData ^= ds.isOperatorControl() ? ref[3] : 0; // (Operator Control, )
    miscData ^= ds.isTest() ? ref[4] : 0; // (Test, )
    miscData ^= ds.isFMSAttached() ? ref[5] : 0; // (FMS, )
    byte [] data = new byte[14 + headerSize];
    int pos = 0;
    generateHeader(data, 0, headerSize, 0);
    pos += headerSize;
    System.arraycopy(matchTimeRaw, 0, data, pos, 4); pos += 4;
    System.arraycopy(batteryVoltageRaw, 0, data, pos, 4); pos += 4;
    data[pos] = dsNumber; pos++;
    System.arraycopy(teamNumberRaw, 0, data, pos, 4); pos += 4;
    data[pos] = miscData; pos++;
    return data;
}

private static byte [] createGamePacket() {
    DriverStation ds = DriverStation.getInstance();
    float matchTime = (float)ds.getMatchTime();
    float batteryVoltage = (float)ds.getBatteryVoltage();
    byte dsNumber = (byte)ds.getLocation();
    int teamNumber = ds.getTeamNumber();
    byte [] matchTimeRaw = intToByteArray(Float.floatToIntBits(matchTime));
    byte [] batteryVoltageRaw = intToByteArray(Float.floatToIntBits(batteryVoltage));
    byte [] teamNumberRaw = intToByteArray(teamNumber);
    byte miscData = 0;
    final byte [] ref = {(byte)0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01};
    miscData ^= (ds.getAlliance().value==Alliance.kBlue_val)?ref[0]:0; //(Blue, Red)(True, False)
    miscData ^= !ds.isEnabled() ? ref[1] : 0; // (Disabled, Enabled)
    miscData ^= ds.isAutonomous()? ref[2] : 0; // (Autonomous, )
    miscData ^= ds.isOperatorControl() ? ref[3] : 0; // (Operator Control, )
    miscData ^= ds.isTest() ? ref[4] : 0; // (Test, )
    miscData ^= ds.isFMSAttached() ? ref[5] : 0; // (FMS, )
    byte [] data = new byte[14 + headerSize];
    int pos = 0;
    generateHeader(data, 0, headerSize, 0);
    pos += headerSize;
    System.arraycopy(matchTimeRaw, 0, data, pos, 4); pos += 4;
    System.arraycopy(batteryVoltageRaw, 0, data, pos, 4); pos += 4;
    data[pos] = dsNumber; pos++;
    System.arraycopy(teamNumberRaw, 0, data, pos, 4); pos += 4;
    data[pos] = miscData; pos++;
    return data;
}

public void update() {
    if (Robot.ds.getAlliance().equals(Alliance.Blue)) {
        ledBlueAlliance = true;
        ledRedAlliance = false;
    } else if (Robot.ds.getAlliance().equals(Alliance.Red)) {
        ledBlueAlliance = false;
        ledRedAlliance = true;
    } else {
        ledBlueAlliance = false;
        ledRedAlliance = false;
    }
    double angle = table.getNumber("p_angle", 0);
    double targets = table.getNumber("targets", 0);
    if (targets < 2)
        vision = 0;
    else if (angle > 1)
        vision = 1;
    else if (angle < -1)
        vision = 3;
    else
        vision = 2;

    table.putNumber("vision", vision);
    LEDZ.putNumber("ledGearOn", ledGearOn);
    byte[] ff = new byte[1];
    // if(ledStatus != 0){
    // Robot.driveTrain.tankDrive(0, 0.25);
    // }

    if (!Robot.gear.getHaltGear()) {
        ff[0] = (byte) 255;
        ledOut.write(ff, 1);
    } else if (vision == 1) {
        ff[0] = (byte) 110;
        ledOut.write(ff, 1);
    } else if (vision == 2) {
        ff[0] = (byte) 140;
        ledOut.write(ff, 1);
    } else if (vision == 3) {
        ff[0] = (byte) 120;
        ledOut.write(ff, 1);
    } else if (ledGearOn == 1) {
        ff[0] = (byte) 130;
        ledOut.write(ff, 1);
    }
    // else if(ledGearOn == 0){
    // ff[0] = (byte) 255;
    // ledOut.write(ff, 1);
    // }
    // else{
    // ff[0] = (byte) 130;
    // ledOut.write(ff, 1);
    // }
    else if (ledHang) {
        ff[0] = (byte) 253;
        ledOut.write(ff, 1);
    } else if (ledBlueAlliance) {
        ff[0] = (byte) 10;
        ledOut.write(ff, 1);
    } else if (ledRedAlliance) {
        ff[0] = (byte) 20;
        ledOut.write(ff, 1);
    } else {
        ff[0] = (byte) 0;
        ledOut.write(ff, 1);
    }
    // if(ledOff){
    // ff[0] = (byte) 0;
    // ledOut.write(ff, 1);
    // }
}

/**
 * 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;
}

public static Alliance getAlliance() {
    return (dsSet() ? ds.getAlliance() : Alliance.Invalid);
}

public static boolean isRed() {
    return driverStation.getAlliance().value == Alliance.kRed_val;
}

/**
 * Updates the pattern that the LEDs are displaying. This method checks the
 * status of various robot components to determine what pattern to display.
 */
public void updatePattern() {
    int oldPattern = pattern;
    int oldOffset = offset;

    // LEDs are solid by default
    pattern = SOLID_CODE;
    DriverStation ds = DriverStation.getInstance();
    if (ds.isEnabled()) {
        // Enabled
        if (ds.isAutonomous()) {
            // Autonomous mode
            pattern = RANDOM_PATTERN_CODE;
        } else if (ds.isOperatorControl()) {
            // Teleop
            if (Robot.catcherSubsytem.isExtended()) {
                // Catcher extended
                pattern = BLINK_CODE;
            } else {
                switch (Robot.sweeperSubsystem.getState().state) {
                    case SweeperSubsystem.MotorState.SWEEPING_VALUE:
                        // Sweeper sweeping
                        pattern = DIVERGE_CODE;
                        break;
                    case SweeperSubsystem.MotorState.EJECTING_VALUE:
                        // Sweeper ejecting
                        pattern = CONVERGE_CODE;
                        break;
                    default:
                    case SweeperSubsystem.MotorState.OFF_VALUE:
                        // Sweeper off
                        if (Robot.sweeperSubsystem.isExtended()) {
                            // Sweeper extended
                            pattern = PULSE_CODE;
                        }
                        break;
                }
            }
        }
    }

    // Update the color offset based on the current alliance.
    switch (DriverStation.getInstance().getAlliance().value) {
        case Alliance.kRed_val:
            offset = RED_OFFSET;
            break;
        case Alliance.kBlue_val:
            offset = BLUE_OFFSET;
            break;
        default:
        case Alliance.kInvalid_val:
            // If the alliance is unknown (ie. not connected to FMS), make 
            // the LEDs green.
            offset = GREEN_OFFSET;
            break;
    }

    // If the pattern or offset has changed, send the new data to the
    // Arduino.
    if (oldPattern != pattern || oldOffset != offset) {
        switch (pattern) {
            // DISABLE_CODE and RANDOM_PATTERN_CODE do not have different
            // colors, so do not factor in the offset.
            case DISABLE_CODE:
            case RANDOM_PATTERN_CODE:
                sendData(pattern);
                break;
            default:
                // Every other code is available in red, blue or green.
                sendData(pattern + offset);
        }
    }
}

public void init() {
    primaryXboxController = new Joystick(PRIMARY_JOY);
    secondaryXboxController = new Joystick(SECONDARY_JOY);

    ALLIANCE_COLOR = DriverStation.getInstance().getAlliance().value;
    SmartDashboard.putBoolean("Alliance", ALLIANCE_COLOR == DriverStation.Alliance.kBlue_val);

    preferencesManagers = BadPreferences.getInstance();

    //button that senses seconadry Right bumper press for shooter injection
    /*if (CommandBase.frisbeePusher != null)
     {
     Button injectFrisbee = new Button() {
     public boolean get()
     {
     return (secondaryXboxController.getRawButton(RB));
     }
     };
     injectFrisbee.whenPressed(new InjectFrisbee());   
     }*/

    //press A to climb
    //if (CommandBase.climberArticulator != null) {

        Button climb = new Button() {
            public boolean get() {
                return (OI.getPrimaryRightTrigger() > 0);
            }
        };
        climb.whenPressed(new ClimbForTenPoints());
    //}

    if (CommandBase.shooterArticulator != null)
    {
        Button aim = new Button()
        {
            public boolean get()
            {
                return (isPrimaryYButtonPressed());
            }
        };
        aim.whenPressed(new AimWithCamera());
    }


    if (!this.CONSOLE_OUTPUT_ENABLED) {
        System.out.println("Console output has been disabled from OI");
    }
}