Java 类edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary 实例源码

/**
 * DriverStation constructor.
 *
 * The single DriverStation instance is created statically with the instance
 * static member variable.
 */
protected DriverStation() {
  m_dataSem = new Object();
  for (int i = 0; i < kJoystickPorts; i++) {
    m_joystickButtons[i] = new HALJoystickButtons();
  }

  m_packetDataAvailableMutex = HALUtil.initializeMutexNormal();
  m_packetDataAvailableSem = HALUtil.initializeMultiWait();
  FRCNetworkCommunicationsLibrary.setNewDataSem(m_packetDataAvailableSem);

  m_thread = new Thread(new DriverStationTask(this), "FRCDriverStation");
  m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2);

  m_thread.start();
}

/**
 * Get the value of the axis on a joystick. This depends on the mapping of the
 * joystick connected to the specified port.
 *
 * @param stick The joystick to read.
 * @param axis The analog axis value to read from the joystick.
 * @return The value of the axis on the joystick.
 */
public synchronized double getStickAxis(int stick, int axis) {
  if (stick < 0 || stick >= kJoystickPorts) {
    throw new RuntimeException("Joystick index is out of range, should be 0-5");
  }

  if (axis < 0 || axis >= FRCNetworkCommunicationsLibrary.kMaxJoystickAxes) {
    throw new RuntimeException("Joystick axis is out of range");
  }

  if (axis >= m_joystickAxes[stick].length) {
    reportJoystickUnpluggedWarning("Joystick axis " + axis + " on port " + stick
        + " not available, check if controller is plugged in");
    return 0.0;
  }

  byte value = (byte) m_joystickAxes[stick][axis];

  if (value < 0) {
    return value / 128.0;
  } else {
    return value / 127.0;
  }
}

/**
 * Get the state of a POV on the joystick.
 *
 * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
 */
public synchronized int getStickPOV(int stick, int pov) {
  if (stick < 0 || stick >= kJoystickPorts) {
    throw new RuntimeException("Joystick index is out of range, should be 0-5");
  }

  if (pov < 0 || pov >= FRCNetworkCommunicationsLibrary.kMaxJoystickPOVs) {
    throw new RuntimeException("Joystick POV is out of range");
  }

  if (pov >= m_joystickPOVs[stick].length) {
    reportJoystickUnpluggedWarning("Joystick POV " + pov + " on port " + stick
        + " not available, check if controller is plugged in");
    return -1;
  }

  return m_joystickPOVs[stick][pov];
}

/**
 * Gets the value of isXbox on a joystick
 *
 * @param stick The joystick port number
 * @return A boolean that returns the value of isXbox
 */
public synchronized boolean getJoystickIsXbox(int stick) {

  if (stick < 0 || stick >= kJoystickPorts) {
    throw new RuntimeException("Joystick index is out of range, should be 0-5");
  }
  // TODO: Remove this when calling for descriptor on empty stick no longer
  // crashes
  if (1 > m_joystickButtons[stick].count && 1 > m_joystickAxes[stick].length) {
    reportJoystickUnpluggedWarning("Joystick on port " + stick
        + " not available, check if controller is plugged in");
    return false;
  }
  boolean retVal = false;
  if (FRCNetworkCommunicationsLibrary.HALGetJoystickIsXbox((byte) stick) == 1) {
    retVal = true;
  }
  return retVal;
}

/**
 * Get the current alliance from the FMS
 *$
 * @return the current alliance
 */
public Alliance getAlliance() {
  HALAllianceStationID allianceStationID =
      FRCNetworkCommunicationsLibrary.HALGetAllianceStation();
  if (allianceStationID == null) {
    return Alliance.Invalid;
  }

  switch (allianceStationID) {
    case Red1:
    case Red2:
    case Red3:
      return Alliance.Red;

    case Blue1:
    case Blue2:
    case Blue3:
      return Alliance.Blue;

    default:
      return Alliance.Invalid;
  }
}

/**
 * Gets the location of the team's driver station controls.
 *
 * @return the location of the team's driver station controls: 1, 2, or 3
 */
public int getLocation() {
  HALAllianceStationID allianceStationID =
      FRCNetworkCommunicationsLibrary.HALGetAllianceStation();
  if (allianceStationID == null) {
    return 0;
  }
  switch (allianceStationID) {
    case Red1:
    case Blue1:
      return 1;

    case Red2:
    case Blue2:
      return 2;

    case Blue3:
    case Red3:
      return 3;

    default:
      return 0;
  }
}

private static void reportErrorImpl(boolean is_error, int code, String error, boolean printTrace) {
  StackTraceElement[] traces = Thread.currentThread().getStackTrace();
  String locString;
  if (traces.length > 3)
    locString = traces[3].toString();
  else
    locString = new String();
  boolean haveLoc = false;
  String traceString = new String();
  traceString = " at ";
  for (int i = 3; i < traces.length; i++) {
    String loc = traces[i].toString();
    traceString += loc + "\n";
    // get first user function
    if (!haveLoc && !loc.startsWith("edu.wpi.first.wpilibj")) {
      locString = loc;
      haveLoc = true;
    }
  }
  FRCNetworkCommunicationsLibrary.HALSendError(is_error, code, false, error, locString, printTrace ? traceString : "", true);
}

/**
 * Get the value of the axis on a joystick.
 * This depends on the mapping of the joystick connected to the specified port.
 *
 * @param stick The joystick to read.
 * @param axis  The analog axis value to read from the joystick.
 * @return The value of the axis on the joystick.
 */
public synchronized double getStickAxis(int stick, int axis) {
    if (stick < 0 || stick >= kJoystickPorts) {
        throw new RuntimeException("Joystick index is out of range, should be 0-5");
    }

    if (axis < 0 || axis >= FRCNetworkCommunicationsLibrary.kMaxJoystickAxes) {
        throw new RuntimeException("Joystick axis is out of range");
    }

    if (axis >= m_joystickAxes[stick].length) {
        reportJoystickUnpluggedError("WARNING: Joystick axis " + axis + " on port " + stick + " not available, check if controller is plugged in\n");
        return 0.0;
    }

    byte value = (byte) m_joystickAxes[stick][axis];

    if (value < 0) {
        return value / 128.0;
    } else {
        return value / 127.0;
    }
}

/**
 * Get the state of a POV on the joystick.
 *
 * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
 */
public synchronized int getStickPOV(int stick, int pov) {
    if (stick < 0 || stick >= kJoystickPorts) {
        throw new RuntimeException("Joystick index is out of range, should be 0-5");
    }

    if (pov < 0 || pov >= FRCNetworkCommunicationsLibrary.kMaxJoystickPOVs) {
        throw new RuntimeException("Joystick POV is out of range");
    }

    if (pov >= m_joystickPOVs[stick].length) {
        reportJoystickUnpluggedError("WARNING: Joystick POV " + pov + " on port " + stick + " not available, check if controller is plugged in\n");
        return 0;
    }

    return m_joystickPOVs[stick][pov];
}

/**
 * Get the current alliance from the FMS
 *
 * @return the current alliance
 */
public Alliance getAlliance() {
    HALAllianceStationID allianceStationID = FRCNetworkCommunicationsLibrary.HALGetAllianceStation();
    if (allianceStationID == null) {
        return Alliance.Invalid;
    }

    switch (allianceStationID) {
        case Red1:
        case Red2:
        case Red3:
            return Alliance.Red;

        case Blue1:
        case Blue2:
        case Blue3:
            return Alliance.Blue;

        default:
            return Alliance.Invalid;
    }
}

/**
 * Gets the location of the team's driver station controls.
 *
 * @return the location of the team's driver station controls: 1, 2, or 3
 */
public int getLocation() {
    HALAllianceStationID allianceStationID = FRCNetworkCommunicationsLibrary.HALGetAllianceStation();
    if (allianceStationID == null) {
        return 0;
    }
    switch (allianceStationID) {
        case Red1:
        case Blue1:
            return 1;

        case Red2:
        case Blue2:
            return 2;

        case Blue3:
        case Red3:
            return 3;

        default:
            return 0;
    }
}

/**
 * Report error to Driver Station.
 * Also prints error to System.err
 * Optionally appends Stack trace to error message
 *
 * @param printTrace If true, append stack trace to error string
 */
public static void reportError(String error, boolean printTrace) {
    String errorString = error;
    if (printTrace) {
        errorString += " at ";
        StackTraceElement[] traces = Thread.currentThread().getStackTrace();
        for (int i = 2; i < traces.length; i++) {
            errorString += traces[i].toString() + "\n";
        }
    }
    System.err.println(errorString);
    HALControlWord controlWord = FRCNetworkCommunicationsLibrary.HALGetControlWord();
    if (controlWord.getDSAttached()) {
        FRCNetworkCommunicationsLibrary.HALSetErrorData(errorString);
    }
}

/**
 * Set the rumble output for the joystick. The DS currently supports 2 rumble
 * values, left rumble and right rumble
 *$
 * @param type Which rumble value to set
 * @param value The normalized value (0 to 1) to set the rumble to
 */
public void setRumble(RumbleType type, float value) {
  if (value < 0)
    value = 0;
  else if (value > 1)
    value = 1;
  if (type.value == RumbleType.kLeftRumble_val)
    m_leftRumble = (short) (value * 65535);
  else
    m_rightRumble = (short) (value * 65535);
  FRCNetworkCommunicationsLibrary.HALSetJoystickOutputs((byte) m_port, m_outputs, m_leftRumble,
      m_rightRumble);
}

/**
 * Set a single HID output value for the joystick.
 *$
 * @param outputNumber The index of the output to set (1-32)
 * @param value The value to set the output to
 */

public void setOutput(int outputNumber, boolean value) {
  m_outputs = (m_outputs & ~(1 << (outputNumber - 1))) | ((value ? 1 : 0) << (outputNumber - 1));
  FRCNetworkCommunicationsLibrary.HALSetJoystickOutputs((byte) m_port, m_outputs, m_leftRumble,
      m_rightRumble);
}

/**
 * Common initialization for all robot programs.
 */
public static void initializeHardwareConfiguration() {
  FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationReserve();

  // Set some implementations so that the static methods work properly
  Timer.SetImplementation(new HardwareTimer());
  HLUsageReporting.SetImplementation(new HardwareHLUsageReporting());
  RobotState.SetImplementation(DriverStation.getInstance());
}

/**
 * Provides the service routine for the DS polling thread.
 */
private void task() {
  int safetyCounter = 0;
  while (m_thread_keepalive) {
    HALUtil.takeMultiWait(m_packetDataAvailableSem, m_packetDataAvailableMutex);
    synchronized (this) {
      getData();
    }
    synchronized (m_dataSem) {
      m_dataSem.notifyAll();
    }
    if (++safetyCounter >= 4) {
      MotorSafetyHelper.checkMotors();
      safetyCounter = 0;
    }
    if (m_userInDisabled) {
      FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramDisabled();
    }
    if (m_userInAutonomous) {
      FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramAutonomous();
    }
    if (m_userInTeleop) {
      FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramTeleop();
    }
    if (m_userInTest) {
      FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramTest();
    }
  }
}

/**
 * Copy data from the DS task for the user. If no new data exists, it will
 * just be returned, otherwise the data will be copied from the DS polling
 * loop.
 */
protected synchronized void getData() {

  // Get the status of all of the joysticks
  for (byte stick = 0; stick < kJoystickPorts; stick++) {
    m_joystickAxes[stick] = FRCNetworkCommunicationsLibrary.HALGetJoystickAxes(stick);
    m_joystickPOVs[stick] = FRCNetworkCommunicationsLibrary.HALGetJoystickPOVs(stick);
    ByteBuffer countBuffer = ByteBuffer.allocateDirect(1);
    m_joystickButtons[stick].buttons =
        FRCNetworkCommunicationsLibrary.HALGetJoystickButtons((byte) stick, countBuffer);
    m_joystickButtons[stick].count = countBuffer.get();
  }

  m_newControlData = true;
}

/**
 * Gets the value of type on a joystick
 *
 * @param stick The joystick port number
 * @return The value of type
 */
public synchronized int getJoystickType(int stick) {

  if (stick < 0 || stick >= kJoystickPorts) {
    throw new RuntimeException("Joystick index is out of range, should be 0-5");
  }
  // TODO: Remove this when calling for descriptor on empty stick no longer
  // crashes
  if (1 > m_joystickButtons[stick].count && 1 > m_joystickAxes[stick].length) {
    reportJoystickUnpluggedWarning("Joystick on port " + stick
        + " not available, check if controller is plugged in");
    return -1;
  }
  return FRCNetworkCommunicationsLibrary.HALGetJoystickType((byte) stick);
}

/**
 * Gets the name of the joystick at a port
 *
 * @param stick The joystick port number
 * @return The value of name
 */
public synchronized String getJoystickName(int stick) {

  if (stick < 0 || stick >= kJoystickPorts) {
    throw new RuntimeException("Joystick index is out of range, should be 0-5");
  }
  // TODO: Remove this when calling for descriptor on empty stick no longer
  // crashes
  if (1 > m_joystickButtons[stick].count && 1 > m_joystickAxes[stick].length) {
    reportJoystickUnpluggedWarning("Joystick on port " + stick
        + " not available, check if controller is plugged in");
    return "";
  }
  return FRCNetworkCommunicationsLibrary.HALGetJoystickName((byte) stick);
}

/**
 * Provides the service routine for the DS polling thread.
 */
private void task() {
    int safetyCounter = 0;
    while (m_thread_keepalive) {
        try {
            Thread.sleep(25);
        } catch (InterruptedException e) {
            // TODO Auto-generated catch block
            e.printStackTrace();
        }
        synchronized (this) {
            getData();
        }
        synchronized (m_dataSem) {
            m_dataSem.notifyAll();
        }
        if (++safetyCounter >= 4) {
            MotorSafetyHelper.checkMotors();
            safetyCounter = 0;
        }
        if (m_userInDisabled) {
            FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramDisabled();
        }
        if (m_userInAutonomous) {
            FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramAutonomous();
        }
        if (m_userInTeleop) {
            FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramTeleop();
        }
        if (m_userInTest) {
            FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramTest();
        }
    }
}

/**
 * Copy data from the DS task for the user.
 * If no new data exists, it will just be returned, otherwise
 * the data will be copied from the DS polling loop.
 */
protected synchronized void getData() {

    // Get the status of all of the joysticks
    for (byte stick = 0; stick < kJoystickPorts; stick++) {
        m_joystickAxes[stick] = FRCNetworkCommunicationsLibrary.HALGetJoystickAxes(stick);
        m_joystickPOVs[stick] = FRCNetworkCommunicationsLibrary.HALGetJoystickPOVs(stick);
        ByteBuffer countBuffer = ByteBuffer.allocateDirect(1);
        m_joystickButtons[stick].buttons = FRCNetworkCommunicationsLibrary.HALGetJoystickButtons((byte) stick, countBuffer);
        m_joystickButtons[stick].count = countBuffer.get();
    }

    m_newControlData = true;
}

/**
 * Gets a value indicating whether the FPGA outputs are enabled. The outputs may be disabled
 * if the robot is disabled or e-stopped, the watdhog has expired, or if the roboRIO browns out.
 *
 * @return True if the FPGA outputs are enabled.
 */
public boolean isSysActive() {
    ByteBuffer status = ByteBuffer.allocateDirect(4);
    status.order(ByteOrder.LITTLE_ENDIAN);
    boolean retVal = FRCNetworkCommunicationsLibrary.HALGetSystemActive(status.asIntBuffer());
    HALUtil.checkStatus(status.asIntBuffer());
    return retVal;
}

/**
 * Check if the system is browned out.
 *
 * @return True if the system is browned out
 */
public boolean isBrownedOut() {
    ByteBuffer status = ByteBuffer.allocateDirect(4);
    status.order(ByteOrder.LITTLE_ENDIAN);
    boolean retVal = FRCNetworkCommunicationsLibrary.HALGetBrownedOut(status.asIntBuffer());
    HALUtil.checkStatus(status.asIntBuffer());
    return retVal;
}

/**
 * Set all HID output values for the joystick.
 *$
 * @param value The 32 bit output value (1 bit for each output)
 */
public void setOutputs(int value) {
  m_outputs = value;
  FRCNetworkCommunicationsLibrary.HALSetJoystickOutputs((byte) m_port, m_outputs, m_leftRumble,
      m_rightRumble);
}

/**
 * Start a competition. This code tracks the order of the field starting to
 * ensure that everything happens in the right order. Repeatedly run the
 * correct method, either Autonomous or OperatorControl when the robot is
 * enabled. After running the correct method, wait for some state to change,
 * either the other mode starts or the robot is disabled. Then go back and
 * wait for the robot to be enabled again.
 */
public void startCompetition() {
  UsageReporting.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Sample);

  robotInit();

  // Tell the DS that the robot is ready to be enabled
  FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramStarting();

  robotMain();
  if (!m_robotMainOverridden) {
    // first and one-time initialization
    LiveWindow.setEnabled(false);

    while (true) {
      if (isDisabled()) {
        m_ds.InDisabled(true);
        disabled();
        m_ds.InDisabled(false);
        while (isDisabled()) {
          Timer.delay(0.01);
        }
      } else if (isAutonomous()) {
        m_ds.InAutonomous(true);
        autonomous();
        m_ds.InAutonomous(false);
        while (isAutonomous() && !isDisabled()) {
          Timer.delay(0.01);
        }
      } else if (isTest()) {
        LiveWindow.setEnabled(true);
        m_ds.InTest(true);
        test();
        m_ds.InTest(false);
        while (isTest() && isEnabled())
          Timer.delay(0.01);
        LiveWindow.setEnabled(false);
      } else {
        m_ds.InOperatorControl(true);
        operatorControl();
        m_ds.InOperatorControl(false);
        while (isOperatorControl() && !isDisabled()) {
          Timer.delay(0.01);
        }
      }
    } /* while loop */
  }
}

public boolean isDSAttached() {
  HALControlWord controlWord = FRCNetworkCommunicationsLibrary.HALGetControlWord();
  return controlWord.getDSAttached();
}

public boolean isDSAttached() {
    HALControlWord controlWord = FRCNetworkCommunicationsLibrary.HALGetControlWord();
    return controlWord.getDSAttached();
}

/**
 * Start a competition. This code tracks the order of the field starting to
 * ensure that everything happens in the right order. Repeatedly run the
 * correct method, either Autonomous or OperatorControl when the robot is
 * enabled. After running the correct method, wait for some state to change,
 * either the other mode starts or the robot is disabled. Then go back and
 * wait for the robot to be enabled again.
 */
public void startCompetition() {
    UsageReporting.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Sample);

    robotInit();

    // Tell the DS that the robot is ready to be enabled
    FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramStarting();

    // first and one-time initialization
    LiveWindow.setEnabled(false);

    while (true) {
        if (isDisabled()) {
            m_ds.InDisabled(true);
            disabled();
            m_ds.InDisabled(false);
            while (isDisabled()) {
                Timer.delay(0.01);
            }
        } else if (isAutonomous()) {
            m_ds.InAutonomous(true);
            autonomous();
            m_ds.InAutonomous(false);
            while (isAutonomous() && !isDisabled()) {
                Timer.delay(0.01);
            }
        } else if (isTest()) {
            LiveWindow.setEnabled(true);
            m_ds.InTest(true);
            test();
            m_ds.InTest(false);
            while (isTest() && isEnabled())
                Timer.delay(0.01);
            LiveWindow.setEnabled(false);
        } else if (isOperatorControl()) {
            m_ds.InOperatorControl(true);
            operatorControl();
            m_ds.InOperatorControl(false);
            while (isOperatorControl() && !isDisabled()) {
                Timer.delay(0.01);
            }
        } else {
            m_ds.InDisabled(true);
            disabled();
            m_ds.InDisabled(false);
            while (isDisabled()) {
                Timer.delay(0.01);
            }
        }
    } /* while loop */
}

/**
 * Gets a value indicating whether the Driver Station requires the robot to be
 * enabled.
 *
 * @return True if the robot is enabled, false otherwise.
 */
public boolean isEnabled() {
  HALControlWord controlWord = FRCNetworkCommunicationsLibrary.HALGetControlWord();
  return controlWord.getEnabled() && controlWord.getDSAttached();
}

/**
 * Gets a value indicating whether the Driver Station requires the robot to be
 * running in autonomous mode.
 *
 * @return True if autonomous mode should be enabled, false otherwise.
 */
public boolean isAutonomous() {
  HALControlWord controlWord = FRCNetworkCommunicationsLibrary.HALGetControlWord();
  return controlWord.getAutonomous();
}

/**
 * Gets a value indicating whether the Driver Station requires the robot to be
 * running in test mode.
 *$
 * @return True if test mode should be enabled, false otherwise.
 */
public boolean isTest() {
  HALControlWord controlWord = FRCNetworkCommunicationsLibrary.HALGetControlWord();
  return controlWord.getTest();
}

/**
 * Gets a value indicating whether the FPGA outputs are enabled. The outputs
 * may be disabled if the robot is disabled or e-stopped, the watdhog has
 * expired, or if the roboRIO browns out.
 *
 * @return True if the FPGA outputs are enabled.
 */
public boolean isSysActive() {
  return FRCNetworkCommunicationsLibrary.HALGetSystemActive();
}

/**
 * Check if the system is browned out.
 *$
 * @return True if the system is browned out
 */
public boolean isBrownedOut() {
  return FRCNetworkCommunicationsLibrary.HALGetBrownedOut();
}

/**
 * Is the driver station attached to a Field Management System? Note: This
 * does not work with the Blue DS.
 *$
 * @return True if the robot is competing on a field being controlled by a
 *         Field Management System
 */
public boolean isFMSAttached() {
  HALControlWord controlWord = FRCNetworkCommunicationsLibrary.HALGetControlWord();
  return controlWord.getFMSAttached();
}

/**
 * Return the approximate match time The FMS does not send an official match
 * time to the robots, but does send an approximate match time. The value will
 * count down the time remaining in the current period (auto or teleop).
 * Warning: This is not an official time (so it cannot be used to dispute ref
 * calls or guarantee that a function will trigger before the match ends) The
 * Practice Match function of the DS approximates the behaviour seen on the
 * field.
 *$
 * @return Time remaining in current match period (auto or teleop) in seconds
 */
public double getMatchTime() {
  return FRCNetworkCommunicationsLibrary.HALGetMatchTime();
}

/**
 * Is the driver station attached to a Field Management System?
 * Note: This does not work with the Blue DS.
 *
 * @return True if the robot is competing on a field being controlled by a Field Management System
 */
public boolean isFMSAttached() {
    HALControlWord controlWord = FRCNetworkCommunicationsLibrary.HALGetControlWord();
    return controlWord.getFMSAttached();
}

/**
 * Return the approximate match time
 * The FMS does not send an official match time to the robots, but does send an approximate match time.
 * The value will count down the time remaining in the current period (auto or teleop).
 * Warning: This is not an official time (so it cannot be used to dispute ref calls or guarantee that a function
 * will trigger before the match ends)
 * The Practice Match function of the DS approximates the behaviour seen on the field.
 *
 * @return Time remaining in current match period (auto or teleop) in seconds
 */
public double getMatchTime() {
    return FRCNetworkCommunicationsLibrary.HALGetMatchTime();
}