SCRIPT Move cc files

wpilibc/src/main/native/cpp/driverstation/DSControlWord.cpp

@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/DSControlWord.h"
+
+#include <hal/DriverStation.h>
+
+using namespace frc;
+
+DSControlWord::DSControlWord() {
+  HAL_GetControlWord(&m_controlWord);
+}
+
+bool DSControlWord::IsEnabled() const {
+  return m_controlWord.enabled && m_controlWord.dsAttached;
+}
+
+bool DSControlWord::IsDisabled() const {
+  return !(m_controlWord.enabled && m_controlWord.dsAttached);
+}
+
+bool DSControlWord::IsEStopped() const {
+  return m_controlWord.eStop;
+}
+
+bool DSControlWord::IsAutonomous() const {
+  return m_controlWord.autonomous;
+}
+
+bool DSControlWord::IsAutonomousEnabled() const {
+  return m_controlWord.autonomous && m_controlWord.enabled &&
+         m_controlWord.dsAttached;
+}
+
+bool DSControlWord::IsTeleop() const {
+  return !(m_controlWord.autonomous || m_controlWord.test);
+}
+
+bool DSControlWord::IsTeleopEnabled() const {
+  return !m_controlWord.autonomous && !m_controlWord.test &&
+         m_controlWord.enabled && m_controlWord.dsAttached;
+}
+
+bool DSControlWord::IsTest() const {
+  return m_controlWord.test;
+}
+
+bool DSControlWord::IsDSAttached() const {
+  return m_controlWord.dsAttached;
+}
+
+bool DSControlWord::IsFMSAttached() const {
+  return m_controlWord.fmsAttached;
+}

wpilibc/src/main/native/cpp/driverstation/DriverStation.cpp

@@ -0,0 +1,908 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/DriverStation.h"
+
+#include <stdint.h>
+
+#include <array>
+#include <atomic>
+#include <memory>
+#include <span>
+#include <string>
+#include <string_view>
+
+#include <fmt/format.h>
+#include <hal/DriverStation.h>
+#include <hal/DriverStationTypes.h>
+#include <hal/HALBase.h>
+#include <hal/Power.h>
+#include <networktables/BooleanTopic.h>
+#include <networktables/IntegerTopic.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StringTopic.h>
+#include <wpi/EventVector.h>
+#include <wpi/condition_variable.h>
+#include <wpi/datalog/DataLog.h>
+#include <wpi/json.h>
+#include <wpi/mutex.h>
+#include <wpi/timestamp.h>
+
+#include "frc/Errors.h"
+#include "frc/Timer.h"
+
+using namespace frc;
+
+static constexpr int availableToCount(uint64_t available) {
+  return 64 - std::countl_zero(available);
+}
+
+namespace {
+// A simple class which caches the previous value written to an NT entry
+// Used to prevent redundant, repeated writes of the same value
+template <typename Topic>
+class MatchDataSenderEntry {
+ public:
+  MatchDataSenderEntry(const std::shared_ptr<nt::NetworkTable>& table,
+                       std::string_view key,
+                       typename Topic::ParamType initialVal,
+                       wpi::json topicProperties = wpi::json::object())
+      : publisher{Topic{table->GetTopic(key)}.PublishEx(Topic::kTypeString,
+                                                        topicProperties)},
+        prevVal{initialVal} {
+    publisher.Set(initialVal);
+  }
+
+  void Set(typename Topic::ParamType val) {
+    if (val != prevVal) {
+      publisher.Set(val);
+      prevVal = val;
+    }
+  }
+
+ private:
+  typename Topic::PublisherType publisher;
+  typename Topic::ValueType prevVal;
+};
+
+static constexpr std::string_view kSmartDashboardType = "FMSInfo";
+
+struct MatchDataSender {
+  std::shared_ptr<nt::NetworkTable> table =
+      nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo");
+  MatchDataSenderEntry<nt::StringTopic> typeMetaData{
+      table,
+      ".type",
+      kSmartDashboardType,
+      {{"SmartDashboard", kSmartDashboardType}}};
+  MatchDataSenderEntry<nt::StringTopic> gameSpecificMessage{
+      table, "GameSpecificMessage", ""};
+  MatchDataSenderEntry<nt::StringTopic> eventName{table, "EventName", ""};
+  MatchDataSenderEntry<nt::IntegerTopic> matchNumber{table, "MatchNumber", 0};
+  MatchDataSenderEntry<nt::IntegerTopic> replayNumber{table, "ReplayNumber", 0};
+  MatchDataSenderEntry<nt::IntegerTopic> matchType{table, "MatchType", 0};
+  MatchDataSenderEntry<nt::BooleanTopic> alliance{table, "IsRedAlliance", true};
+  MatchDataSenderEntry<nt::IntegerTopic> station{table, "StationNumber", 1};
+  MatchDataSenderEntry<nt::IntegerTopic> controlWord{table, "FMSControlData",
+                                                     0};
+};
+
+class JoystickLogSender {
+ public:
+  void Init(wpi::log::DataLog& log, unsigned int stick, int64_t timestamp);
+  void Send(uint64_t timestamp);
+
+ private:
+  void AppendButtons(HAL_JoystickButtons buttons, uint64_t timestamp);
+  void AppendPOVs(const HAL_JoystickPOVs& povs, uint64_t timestamp);
+
+  unsigned int m_stick;
+  HAL_JoystickButtons m_prevButtons;
+  HAL_JoystickAxes m_prevAxes;
+  HAL_JoystickPOVs m_prevPOVs;
+  wpi::log::BooleanArrayLogEntry m_logButtons;
+  wpi::log::FloatArrayLogEntry m_logAxes;
+  wpi::log::IntegerArrayLogEntry m_logPOVs;
+};
+
+class DataLogSender {
+ public:
+  void Init(wpi::log::DataLog& log, bool logJoysticks, int64_t timestamp);
+  void Send(uint64_t timestamp);
+
+ private:
+  std::atomic_bool m_initialized{false};
+
+  HAL_ControlWord m_prevControlWord;
+  wpi::log::BooleanLogEntry m_logEnabled;
+  wpi::log::BooleanLogEntry m_logAutonomous;
+  wpi::log::BooleanLogEntry m_logTest;
+  wpi::log::BooleanLogEntry m_logEstop;
+
+  bool m_logJoysticks;
+  std::array<JoystickLogSender, DriverStation::kJoystickPorts> m_joysticks;
+};
+
+struct Instance {
+  Instance();
+  ~Instance();
+
+  wpi::EventVector refreshEvents;
+  MatchDataSender matchDataSender;
+  std::atomic<DataLogSender*> dataLogSender{nullptr};
+
+  // Joystick button rising/falling edge flags
+  wpi::mutex buttonEdgeMutex;
+  std::array<HAL_JoystickButtons, DriverStation::kJoystickPorts>
+      previousButtonStates;
+  std::array<uint32_t, DriverStation::kJoystickPorts> joystickButtonsPressed;
+  std::array<uint32_t, DriverStation::kJoystickPorts> joystickButtonsReleased;
+
+  bool silenceJoystickWarning = false;
+
+  // Robot state status variables
+  bool userInDisabled = false;
+  bool userInAutonomous = false;
+  bool userInTeleop = false;
+  bool userInTest = false;
+
+  units::second_t nextMessageTime = 0_s;
+};
+}  // namespace
+
+static constexpr auto kJoystickUnpluggedMessageInterval = 1_s;
+
+static Instance& GetInstance() {
+  static Instance instance;
+  return instance;
+}
+
+static void SendMatchData();
+
+template <typename S, typename... Args>
+static inline void ReportJoystickUnpluggedError(const S& format,
+                                                Args&&... args) {
+  ReportJoystickUnpluggedErrorV(format, fmt::make_format_args(args...));
+}
+
+/**
+ * Reports errors related to unplugged joysticks.
+ *
+ * Throttles the errors so that they don't overwhelm the DS.
+ */
+static void ReportJoystickUnpluggedWarningV(fmt::string_view format,
+                                            fmt::format_args args);
+
+template <typename S, typename... Args>
+static inline void ReportJoystickUnpluggedWarning(const S& format,
+                                                  Args&&... args) {
+  ReportJoystickUnpluggedWarningV(format, fmt::make_format_args(args...));
+}
+
+Instance::Instance() {
+  HAL_Initialize(500, 0);
+
+  // All joysticks should default to having zero axes, povs and buttons, so
+  // uninitialized memory doesn't get sent to motor controllers.
+  for (unsigned int i = 0; i < DriverStation::kJoystickPorts; i++) {
+    joystickButtonsPressed[i] = 0;
+    joystickButtonsReleased[i] = 0;
+    previousButtonStates[i].available = 0;
+    previousButtonStates[i].buttons = 0;
+  }
+}
+
+Instance::~Instance() {
+  if (dataLogSender) {
+    delete dataLogSender.load();
+  }
+}
+
+bool DriverStation::GetStickButton(int stick, int button) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+    return false;
+  }
+  if (button < 0 || button >= 64) {
+    FRC_ReportError(warn::BadJoystickIndex, "button {} out of range", button);
+    return false;
+  }
+
+  uint64_t mask = 1LLU << button;
+
+  HAL_JoystickButtons buttons;
+  HAL_GetJoystickButtons(stick, &buttons);
+
+  if ((buttons.available & mask) == 0) {
+    ReportJoystickUnpluggedWarning(
+        "Joystick Button {} missing (available {}), check if all controllers "
+        "are "
+        "plugged in",
+        button, buttons.available);
+    return false;
+  }
+
+  return (buttons.buttons & mask) != 0;
+}
+
+std::optional<bool> DriverStation::GetStickButtonIfAvailable(int stick,
+                                                             int button) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+    return false;
+  }
+  if (button < 0 || button >= 64) {
+    FRC_ReportError(warn::BadJoystickIndex, "button {} out of range", button);
+    return false;
+  }
+
+  uint64_t mask = 1LLU << button;
+
+  HAL_JoystickButtons buttons;
+  HAL_GetJoystickButtons(stick, &buttons);
+
+  if ((buttons.available & mask) == 0) {
+    return std::nullopt;
+  }
+
+  return (buttons.buttons & mask) != 0;
+}
+
+bool DriverStation::GetStickButtonPressed(int stick, int button) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+    return false;
+  }
+  if (button < 0 || button >= 64) {
+    FRC_ReportError(warn::BadJoystickIndex, "button {} out of range", button);
+    return false;
+  }
+
+  HAL_JoystickButtons buttons;
+  HAL_GetJoystickButtons(stick, &buttons);
+
+  uint64_t mask = 1LLU << button;
+
+  if ((buttons.available & mask) == 0) {
+    ReportJoystickUnpluggedWarning(
+        "Joystick Button {} missing (available {}), check if all controllers "
+        "are "
+        "plugged in",
+        button, buttons.available);
+    return false;
+  }
+  auto& inst = ::GetInstance();
+  std::unique_lock lock(inst.buttonEdgeMutex);
+  // If button was pressed, clear flag and return true
+  if (inst.joystickButtonsPressed[stick] & mask) {
+    inst.joystickButtonsPressed[stick] &= ~mask;
+    return true;
+  }
+  return false;
+}
+
+bool DriverStation::GetStickButtonReleased(int stick, int button) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+    return false;
+  }
+  if (button < 0 || button >= 64) {
+    FRC_ReportError(warn::BadJoystickIndex, "button {} out of range", button);
+    return false;
+  }
+
+  HAL_JoystickButtons buttons;
+  HAL_GetJoystickButtons(stick, &buttons);
+
+  uint64_t mask = 1LLU << button;
+
+  if ((buttons.available & mask) == 0) {
+    ReportJoystickUnpluggedWarning(
+        "Joystick Button {} missing (available {}), check if all controllers "
+        "are "
+        "plugged in",
+        button, buttons.available);
+    return false;
+  }
+  auto& inst = ::GetInstance();
+  std::unique_lock lock(inst.buttonEdgeMutex);
+  // If button was released, clear flag and return true
+  if (inst.joystickButtonsReleased[stick] & mask) {
+    inst.joystickButtonsReleased[stick] &= ~mask;
+    return true;
+  }
+  return false;
+}
+
+double DriverStation::GetStickAxis(int stick, int axis) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+    return 0.0;
+  }
+  if (axis < 0 || axis >= HAL_kMaxJoystickAxes) {
+    FRC_ReportError(warn::BadJoystickAxis, "axis {} out of range", axis);
+    return 0.0;
+  }
+
+  uint16_t mask = 1 << axis;
+
+  HAL_JoystickAxes axes;
+  HAL_GetJoystickAxes(stick, &axes);
+
+  if ((axes.available & mask) == 0) {
+    ReportJoystickUnpluggedWarning(
+        "Joystick Axis {} missing (available {}), check if all controllers are "
+        "plugged in",
+        axis, axes.available);
+    return 0.0;
+  }
+
+  return axes.axes[axis];
+}
+
+std::optional<double> DriverStation::GetStickAxisIfAvailable(int stick,
+                                                             int axis) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+    return 0.0;
+  }
+  if (axis < 0 || axis >= HAL_kMaxJoystickAxes) {
+    FRC_ReportError(warn::BadJoystickAxis, "axis {} out of range", axis);
+    return 0.0;
+  }
+
+  uint16_t mask = 1 << axis;
+
+  HAL_JoystickAxes axes;
+  HAL_GetJoystickAxes(stick, &axes);
+
+  if ((axes.available & mask) == 0) {
+    return std::nullopt;
+  }
+
+  return axes.axes[axis];
+}
+
+DriverStation::POVDirection DriverStation::GetStickPOV(int stick, int pov) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+    return kCenter;
+  }
+  if (pov < 0 || pov >= HAL_kMaxJoystickPOVs) {
+    FRC_ReportError(warn::BadJoystickAxis, "POV {} out of range", pov);
+    return kCenter;
+  }
+
+  uint16_t mask = 1 << pov;
+
+  HAL_JoystickPOVs povs;
+  HAL_GetJoystickPOVs(stick, &povs);
+
+  if ((povs.available & mask) == 0) {
+    ReportJoystickUnpluggedWarning(
+        "Joystick POV {} missing (available {}), check if all controllers are "
+        "plugged in",
+        pov, povs.available);
+    return kCenter;
+  }
+
+  return static_cast<POVDirection>(povs.povs[pov]);
+}
+
+uint64_t DriverStation::GetStickButtons(int stick) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+    return 0;
+  }
+
+  HAL_JoystickButtons buttons;
+  HAL_GetJoystickButtons(stick, &buttons);
+
+  return buttons.buttons;
+}
+
+int DriverStation::GetStickAxesMaximumIndex(int stick) {
+  return availableToCount(GetStickAxesAvailable(stick));
+}
+
+int DriverStation::GetStickAxesAvailable(int stick) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+    return 0;
+  }
+
+  HAL_JoystickAxes axes;
+  HAL_GetJoystickAxes(stick, &axes);
+
+  return axes.available;
+}
+
+int DriverStation::GetStickPOVsMaximumIndex(int stick) {
+  return availableToCount(GetStickPOVsAvailable(stick));
+}
+
+int DriverStation::GetStickPOVsAvailable(int stick) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+    return 0;
+  }
+
+  HAL_JoystickPOVs povs;
+  HAL_GetJoystickPOVs(stick, &povs);
+
+  return povs.available;
+}
+
+int DriverStation::GetStickButtonsMaximumIndex(int stick) {
+  return availableToCount(GetStickButtonsAvailable(stick));
+}
+
+uint64_t DriverStation::GetStickButtonsAvailable(int stick) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+    return 0;
+  }
+
+  HAL_JoystickButtons buttons;
+  HAL_GetJoystickButtons(stick, &buttons);
+
+  return buttons.available;
+}
+
+bool DriverStation::GetJoystickIsGamepad(int stick) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+    return false;
+  }
+
+  HAL_JoystickDescriptor descriptor;
+  HAL_GetJoystickDescriptor(stick, &descriptor);
+
+  return static_cast<bool>(descriptor.isGamepad);
+}
+
+int DriverStation::GetJoystickType(int stick) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+    return -1;
+  }
+
+  HAL_JoystickDescriptor descriptor;
+  HAL_GetJoystickDescriptor(stick, &descriptor);
+
+  return static_cast<int>(descriptor.type);
+}
+
+std::string DriverStation::GetJoystickName(int stick) {
+  if (stick < 0 || stick >= kJoystickPorts) {
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
+  }
+
+  HAL_JoystickDescriptor descriptor;
+  HAL_GetJoystickDescriptor(stick, &descriptor);
+
+  return descriptor.name;
+}
+
+bool DriverStation::IsJoystickConnected(int stick) {
+  return GetStickAxesAvailable(stick) != 0 ||
+         GetStickButtonsAvailable(stick) != 0 ||
+         GetStickPOVsAvailable(stick) != 0;
+}
+
+bool DriverStation::IsEnabled() {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return controlWord.enabled && controlWord.dsAttached;
+}
+
+bool DriverStation::IsDisabled() {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return !(controlWord.enabled && controlWord.dsAttached);
+}
+
+bool DriverStation::IsEStopped() {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return controlWord.eStop;
+}
+
+bool DriverStation::IsAutonomous() {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return controlWord.autonomous;
+}
+
+bool DriverStation::IsAutonomousEnabled() {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return controlWord.autonomous && controlWord.enabled;
+}
+
+bool DriverStation::IsTeleop() {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return !(controlWord.autonomous || controlWord.test);
+}
+
+bool DriverStation::IsTeleopEnabled() {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return !controlWord.autonomous && !controlWord.test && controlWord.enabled;
+}
+
+bool DriverStation::IsTest() {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return controlWord.test;
+}
+
+bool DriverStation::IsTestEnabled() {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return controlWord.test && controlWord.enabled;
+}
+
+bool DriverStation::IsDSAttached() {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return controlWord.dsAttached;
+}
+
+bool DriverStation::IsFMSAttached() {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return controlWord.fmsAttached;
+}
+
+std::string DriverStation::GetGameSpecificMessage() {
+  HAL_MatchInfo info;
+  HAL_GetMatchInfo(&info);
+  return std::string(reinterpret_cast<char*>(info.gameSpecificMessage),
+                     info.gameSpecificMessageSize);
+}
+
+std::string DriverStation::GetEventName() {
+  HAL_MatchInfo info;
+  HAL_GetMatchInfo(&info);
+  return info.eventName;
+}
+
+DriverStation::MatchType DriverStation::GetMatchType() {
+  HAL_MatchInfo info;
+  HAL_GetMatchInfo(&info);
+  return static_cast<DriverStation::MatchType>(info.matchType);
+}
+
+int DriverStation::GetMatchNumber() {
+  HAL_MatchInfo info;
+  HAL_GetMatchInfo(&info);
+  return info.matchNumber;
+}
+
+int DriverStation::GetReplayNumber() {
+  HAL_MatchInfo info;
+  HAL_GetMatchInfo(&info);
+  return info.replayNumber;
+}
+
+std::optional<DriverStation::Alliance> DriverStation::GetAlliance() {
+  int32_t status = 0;
+  auto allianceStationID = HAL_GetAllianceStation(&status);
+  switch (allianceStationID) {
+    case HAL_AllianceStationID_kRed1:
+    case HAL_AllianceStationID_kRed2:
+    case HAL_AllianceStationID_kRed3:
+      return kRed;
+    case HAL_AllianceStationID_kBlue1:
+    case HAL_AllianceStationID_kBlue2:
+    case HAL_AllianceStationID_kBlue3:
+      return kBlue;
+    default:
+      return {};
+  }
+}
+
+std::optional<int> DriverStation::GetLocation() {
+  int32_t status = 0;
+  auto allianceStationID = HAL_GetAllianceStation(&status);
+  switch (allianceStationID) {
+    case HAL_AllianceStationID_kRed1:
+    case HAL_AllianceStationID_kBlue1:
+      return 1;
+    case HAL_AllianceStationID_kRed2:
+    case HAL_AllianceStationID_kBlue2:
+      return 2;
+    case HAL_AllianceStationID_kRed3:
+    case HAL_AllianceStationID_kBlue3:
+      return 3;
+    default:
+      return {};
+  }
+}
+
+units::second_t DriverStation::GetMatchTime() {
+  int32_t status = 0;
+  return units::second_t{HAL_GetMatchTime(&status)};
+}
+
+double DriverStation::GetBatteryVoltage() {
+  int32_t status = 0;
+  double voltage = HAL_GetVinVoltage(&status);
+  FRC_CheckErrorStatus(status, "getVinVoltage");
+
+  return voltage;
+}
+
+/**
+ * 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.
+ */
+void DriverStation::RefreshData() {
+  HAL_RefreshDSData();
+  auto& inst = ::GetInstance();
+  {
+    // Compute the pressed and released buttons
+    HAL_JoystickButtons currentButtons;
+    std::unique_lock lock(inst.buttonEdgeMutex);
+
+    for (int32_t i = 0; i < DriverStation::kJoystickPorts; i++) {
+      HAL_GetJoystickButtons(i, &currentButtons);
+
+      // If buttons weren't pressed and are now, set flags in m_buttonsPressed
+      inst.joystickButtonsPressed[i] |=
+          ~inst.previousButtonStates[i].buttons & currentButtons.buttons;
+
+      // If buttons were pressed and aren't now, set flags in m_buttonsReleased
+      inst.joystickButtonsReleased[i] |=
+          inst.previousButtonStates[i].buttons & ~currentButtons.buttons;
+
+      inst.previousButtonStates[i] = currentButtons;
+    }
+  }
+
+  inst.refreshEvents.Wakeup();
+
+  SendMatchData();
+  if (auto sender = inst.dataLogSender.load()) {
+    sender->Send(wpi::Now());
+  }
+}
+
+void DriverStation::ProvideRefreshedDataEventHandle(WPI_EventHandle handle) {
+  auto& inst = ::GetInstance();
+  inst.refreshEvents.Add(handle);
+}
+
+void DriverStation::RemoveRefreshedDataEventHandle(WPI_EventHandle handle) {
+  auto& inst = ::GetInstance();
+  inst.refreshEvents.Remove(handle);
+}
+
+void DriverStation::SilenceJoystickConnectionWarning(bool silence) {
+  ::GetInstance().silenceJoystickWarning = silence;
+}
+
+bool DriverStation::IsJoystickConnectionWarningSilenced() {
+  return !IsFMSAttached() && ::GetInstance().silenceJoystickWarning;
+}
+
+void DriverStation::StartDataLog(wpi::log::DataLog& log, bool logJoysticks) {
+  auto& inst = ::GetInstance();
+  // Note: cannot safely replace, because we wouldn't know when to delete the
+  // "old" one. Instead do a compare and exchange with nullptr. We check first
+  // with a simple load to avoid the new in the common case.
+  if (inst.dataLogSender.load()) {
+    return;
+  }
+  DataLogSender* oldSender = nullptr;
+  DataLogSender* newSender = new DataLogSender;
+  inst.dataLogSender.compare_exchange_strong(oldSender, newSender);
+  if (oldSender) {
+    delete newSender;  // already had a sender
+  } else {
+    newSender->Init(log, logJoysticks, wpi::Now());
+  }
+}
+
+void ReportJoystickUnpluggedWarningV(fmt::string_view format,
+                                     fmt::format_args args) {
+  auto& inst = GetInstance();
+  if (DriverStation::IsFMSAttached() || !inst.silenceJoystickWarning) {
+    auto currentTime = Timer::GetTimestamp();
+    if (currentTime > inst.nextMessageTime) {
+      ReportErrorV(warn::Warning, "", 0, "", format, args);
+      inst.nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
+    }
+  }
+}
+
+void SendMatchData() {
+  int32_t status = 0;
+  HAL_AllianceStationID alliance = HAL_GetAllianceStation(&status);
+  bool isRedAlliance = false;
+  int stationNumber = 1;
+  switch (alliance) {
+    case HAL_AllianceStationID::HAL_AllianceStationID_kBlue1:
+      isRedAlliance = false;
+      stationNumber = 1;
+      break;
+    case HAL_AllianceStationID::HAL_AllianceStationID_kBlue2:
+      isRedAlliance = false;
+      stationNumber = 2;
+      break;
+    case HAL_AllianceStationID::HAL_AllianceStationID_kBlue3:
+      isRedAlliance = false;
+      stationNumber = 3;
+      break;
+    case HAL_AllianceStationID::HAL_AllianceStationID_kRed1:
+      isRedAlliance = true;
+      stationNumber = 1;
+      break;
+    case HAL_AllianceStationID::HAL_AllianceStationID_kRed2:
+      isRedAlliance = true;
+      stationNumber = 2;
+      break;
+    default:
+      isRedAlliance = true;
+      stationNumber = 3;
+      break;
+  }
+
+  HAL_MatchInfo tmpDataStore;
+  HAL_GetMatchInfo(&tmpDataStore);
+
+  auto& inst = GetInstance();
+  inst.matchDataSender.alliance.Set(isRedAlliance);
+  inst.matchDataSender.station.Set(stationNumber);
+  inst.matchDataSender.eventName.Set(tmpDataStore.eventName);
+  inst.matchDataSender.gameSpecificMessage.Set(
+      std::string(reinterpret_cast<char*>(tmpDataStore.gameSpecificMessage),
+                  tmpDataStore.gameSpecificMessageSize));
+  inst.matchDataSender.matchNumber.Set(tmpDataStore.matchNumber);
+  inst.matchDataSender.replayNumber.Set(tmpDataStore.replayNumber);
+  inst.matchDataSender.matchType.Set(static_cast<int>(tmpDataStore.matchType));
+
+  HAL_ControlWord ctlWord;
+  HAL_GetControlWord(&ctlWord);
+  int32_t wordInt = 0;
+  std::memcpy(&wordInt, &ctlWord, sizeof(wordInt));
+  inst.matchDataSender.controlWord.Set(wordInt);
+}
+
+void JoystickLogSender::Init(wpi::log::DataLog& log, unsigned int stick,
+                             int64_t timestamp) {
+  m_stick = stick;
+
+  m_logButtons = wpi::log::BooleanArrayLogEntry{
+      log, fmt::format("DS:joystick{}/buttons", stick), timestamp};
+  m_logAxes = wpi::log::FloatArrayLogEntry{
+      log, fmt::format("DS:joystick{}/axes", stick), timestamp};
+  m_logPOVs = wpi::log::IntegerArrayLogEntry{
+      log, fmt::format("DS:joystick{}/povs", stick), timestamp};
+
+  HAL_GetJoystickButtons(m_stick, &m_prevButtons);
+  HAL_GetJoystickAxes(m_stick, &m_prevAxes);
+  HAL_GetJoystickPOVs(m_stick, &m_prevPOVs);
+  AppendButtons(m_prevButtons, timestamp);
+  int axesCount = availableToCount(m_prevAxes.available);
+  m_logAxes.Append(
+      std::span<const float>{m_prevAxes.axes, static_cast<size_t>(axesCount)},
+      timestamp);
+  AppendPOVs(m_prevPOVs, timestamp);
+}
+
+void JoystickLogSender::Send(uint64_t timestamp) {
+  HAL_JoystickButtons buttons;
+  HAL_GetJoystickButtons(m_stick, &buttons);
+  if (buttons.available != m_prevButtons.available ||
+      buttons.buttons != m_prevButtons.buttons) {
+    AppendButtons(buttons, timestamp);
+  }
+  m_prevButtons = buttons;
+
+  HAL_JoystickAxes axes;
+  HAL_GetJoystickAxes(m_stick, &axes);
+  int axesCount = availableToCount(axes.available);
+  if (axes.available != m_prevAxes.available ||
+      std::memcmp(axes.axes, m_prevAxes.axes,
+                  sizeof(axes.axes[0]) * axesCount) != 0) {
+    m_logAxes.Append(
+        std::span<const float>{axes.axes, static_cast<size_t>(axesCount)},
+        timestamp);
+  }
+  m_prevAxes = axes;
+
+  HAL_JoystickPOVs povs;
+  HAL_GetJoystickPOVs(m_stick, &povs);
+  int povsCount = availableToCount(povs.available);
+  if (povs.available != m_prevPOVs.available ||
+      std::memcmp(povs.povs, m_prevPOVs.povs,
+                  sizeof(povs.povs[0]) * povsCount) != 0) {
+    AppendPOVs(povs, timestamp);
+  }
+  m_prevPOVs = povs;
+}
+
+void JoystickLogSender::AppendButtons(HAL_JoystickButtons buttons,
+                                      uint64_t timestamp) {
+  int count = availableToCount(buttons.available);
+  uint8_t buttonsArr[64];
+  for (int i = 0; i < count; ++i) {
+    buttonsArr[i] = (buttons.buttons & (1llu << i)) != 0;
+  }
+  m_logButtons.Append(
+      std::span<const uint8_t>{buttonsArr, static_cast<size_t>(count)},
+      timestamp);
+}
+
+void JoystickLogSender::AppendPOVs(const HAL_JoystickPOVs& povs,
+                                   uint64_t timestamp) {
+  int count = availableToCount(povs.available);
+  int64_t povsArr[HAL_kMaxJoystickPOVs];
+  for (int i = 0; i < count; ++i) {
+    povsArr[i] = povs.povs[i];
+  }
+  m_logPOVs.Append(
+      std::span<const int64_t>{povsArr, static_cast<size_t>(count)}, timestamp);
+}
+
+void DataLogSender::Init(wpi::log::DataLog& log, bool logJoysticks,
+                         int64_t timestamp) {
+  m_logEnabled = wpi::log::BooleanLogEntry{log, "DS:enabled", timestamp};
+  m_logAutonomous = wpi::log::BooleanLogEntry{log, "DS:autonomous", timestamp};
+  m_logTest = wpi::log::BooleanLogEntry{log, "DS:test", timestamp};
+  m_logEstop = wpi::log::BooleanLogEntry{log, "DS:estop", timestamp};
+
+  // append initial control word values
+  HAL_GetControlWord(&m_prevControlWord);
+  m_logEnabled.Append(m_prevControlWord.enabled, timestamp);
+  m_logAutonomous.Append(m_prevControlWord.autonomous, timestamp);
+  m_logTest.Append(m_prevControlWord.test, timestamp);
+  m_logEstop.Append(m_prevControlWord.eStop, timestamp);
+
+  m_logJoysticks = logJoysticks;
+  if (logJoysticks) {
+    unsigned int i = 0;
+    for (auto&& joystick : m_joysticks) {
+      joystick.Init(log, i++, timestamp);
+    }
+  }
+
+  m_initialized = true;
+}
+
+void DataLogSender::Send(uint64_t timestamp) {
+  if (!m_initialized) {
+    return;
+  }
+
+  // append control word value changes
+  HAL_ControlWord ctlWord;
+  HAL_GetControlWord(&ctlWord);
+  if (ctlWord.enabled != m_prevControlWord.enabled) {
+    m_logEnabled.Append(ctlWord.enabled, timestamp);
+  }
+  if (ctlWord.autonomous != m_prevControlWord.autonomous) {
+    m_logAutonomous.Append(ctlWord.autonomous, timestamp);
+  }
+  if (ctlWord.test != m_prevControlWord.test) {
+    m_logTest.Append(ctlWord.test, timestamp);
+  }
+  if (ctlWord.eStop != m_prevControlWord.eStop) {
+    m_logEstop.Append(ctlWord.eStop, timestamp);
+  }
+  m_prevControlWord = ctlWord;
+
+  if (m_logJoysticks) {
+    // append joystick value changes
+    for (auto&& joystick : m_joysticks) {
+      joystick.Send(timestamp);
+    }
+  }
+}

wpilibc/src/main/native/cpp/driverstation/Gamepad.cpp

@@ -0,0 +1,582 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/Gamepad.h"
+
+#include <hal/UsageReporting.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/event/BooleanEvent.h"
+
+using namespace frc;
+
+Gamepad::Gamepad(int port) : GenericHID(port) {
+  HAL_ReportUsage("HID", port, "Gamepad");
+}
+
+double Gamepad::GetLeftX() const {
+  return GetRawAxis(Axis::kLeftX);
+}
+
+double Gamepad::GetLeftY() const {
+  return GetRawAxis(Axis::kLeftY);
+}
+
+double Gamepad::GetRightX() const {
+  return GetRawAxis(Axis::kRightX);
+}
+
+double Gamepad::GetRightY() const {
+  return GetRawAxis(Axis::kRightY);
+}
+
+double Gamepad::GetLeftTriggerAxis() const {
+  return GetRawAxis(Axis::kLeftTrigger);
+}
+
+BooleanEvent Gamepad::LeftTrigger(double threshold, EventLoop* loop) const {
+  return BooleanEvent(loop, [this, threshold] {
+    return this->GetLeftTriggerAxis() > threshold;
+  });
+}
+
+BooleanEvent Gamepad::LeftTrigger(EventLoop* loop) const {
+  return this->LeftTrigger(0.5, loop);
+}
+
+double Gamepad::GetRightTriggerAxis() const {
+  return GetRawAxis(Axis::kRightTrigger);
+}
+
+BooleanEvent Gamepad::RightTrigger(double threshold, EventLoop* loop) const {
+  return BooleanEvent(loop, [this, threshold] {
+    return this->GetRightTriggerAxis() > threshold;
+  });
+}
+
+BooleanEvent Gamepad::RightTrigger(EventLoop* loop) const {
+  return this->RightTrigger(0.5, loop);
+}
+
+bool Gamepad::GetSouthFaceButton() const {
+  return GetRawButton(Button::kSouthFace);
+}
+
+bool Gamepad::GetSouthFaceButtonPressed() {
+  return GetRawButtonPressed(Button::kSouthFace);
+}
+
+bool Gamepad::GetSouthFaceButtonReleased() {
+  return GetRawButtonReleased(Button::kSouthFace);
+}
+
+BooleanEvent Gamepad::SouthFace(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetSouthFaceButton(); });
+}
+
+bool Gamepad::GetEastFaceButton() const {
+  return GetRawButton(Button::kEastFace);
+}
+
+bool Gamepad::GetEastFaceButtonPressed() {
+  return GetRawButtonPressed(Button::kEastFace);
+}
+
+bool Gamepad::GetEastFaceButtonReleased() {
+  return GetRawButtonReleased(Button::kEastFace);
+}
+
+BooleanEvent Gamepad::EastFace(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetEastFaceButton(); });
+}
+
+bool Gamepad::GetWestFaceButton() const {
+  return GetRawButton(Button::kWestFace);
+}
+
+bool Gamepad::GetWestFaceButtonPressed() {
+  return GetRawButtonPressed(Button::kWestFace);
+}
+
+bool Gamepad::GetWestFaceButtonReleased() {
+  return GetRawButtonReleased(Button::kWestFace);
+}
+
+BooleanEvent Gamepad::WestFace(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetWestFaceButton(); });
+}
+
+bool Gamepad::GetNorthFaceButton() const {
+  return GetRawButton(Button::kNorthFace);
+}
+
+bool Gamepad::GetNorthFaceButtonPressed() {
+  return GetRawButtonPressed(Button::kNorthFace);
+}
+
+bool Gamepad::GetNorthFaceButtonReleased() {
+  return GetRawButtonReleased(Button::kNorthFace);
+}
+
+BooleanEvent Gamepad::NorthFace(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetNorthFaceButton(); });
+}
+
+bool Gamepad::GetBackButton() const {
+  return GetRawButton(Button::kBack);
+}
+
+bool Gamepad::GetBackButtonPressed() {
+  return GetRawButtonPressed(Button::kBack);
+}
+
+bool Gamepad::GetBackButtonReleased() {
+  return GetRawButtonReleased(Button::kBack);
+}
+
+BooleanEvent Gamepad::Back(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetBackButton(); });
+}
+
+bool Gamepad::GetGuideButton() const {
+  return GetRawButton(Button::kGuide);
+}
+
+bool Gamepad::GetGuideButtonPressed() {
+  return GetRawButtonPressed(Button::kGuide);
+}
+
+bool Gamepad::GetGuideButtonReleased() {
+  return GetRawButtonReleased(Button::kGuide);
+}
+
+BooleanEvent Gamepad::Guide(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetGuideButton(); });
+}
+
+bool Gamepad::GetStartButton() const {
+  return GetRawButton(Button::kStart);
+}
+
+bool Gamepad::GetStartButtonPressed() {
+  return GetRawButtonPressed(Button::kStart);
+}
+
+bool Gamepad::GetStartButtonReleased() {
+  return GetRawButtonReleased(Button::kStart);
+}
+
+BooleanEvent Gamepad::Start(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetStartButton(); });
+}
+
+bool Gamepad::GetLeftStickButton() const {
+  return GetRawButton(Button::kLeftStick);
+}
+
+bool Gamepad::GetLeftStickButtonPressed() {
+  return GetRawButtonPressed(Button::kLeftStick);
+}
+
+bool Gamepad::GetLeftStickButtonReleased() {
+  return GetRawButtonReleased(Button::kLeftStick);
+}
+
+BooleanEvent Gamepad::LeftStick(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetLeftStickButton(); });
+}
+
+bool Gamepad::GetRightStickButton() const {
+  return GetRawButton(Button::kRightStick);
+}
+
+bool Gamepad::GetRightStickButtonPressed() {
+  return GetRawButtonPressed(Button::kRightStick);
+}
+
+bool Gamepad::GetRightStickButtonReleased() {
+  return GetRawButtonReleased(Button::kRightStick);
+}
+
+BooleanEvent Gamepad::RightStick(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetRightStickButton(); });
+}
+
+bool Gamepad::GetLeftShoulderButton() const {
+  return GetRawButton(Button::kLeftShoulder);
+}
+
+bool Gamepad::GetLeftShoulderButtonPressed() {
+  return GetRawButtonPressed(Button::kLeftShoulder);
+}
+
+bool Gamepad::GetLeftShoulderButtonReleased() {
+  return GetRawButtonReleased(Button::kLeftShoulder);
+}
+
+BooleanEvent Gamepad::LeftShoulder(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetLeftShoulderButton(); });
+}
+
+bool Gamepad::GetRightShoulderButton() const {
+  return GetRawButton(Button::kRightShoulder);
+}
+
+bool Gamepad::GetRightShoulderButtonPressed() {
+  return GetRawButtonPressed(Button::kRightShoulder);
+}
+
+bool Gamepad::GetRightShoulderButtonReleased() {
+  return GetRawButtonReleased(Button::kRightShoulder);
+}
+
+BooleanEvent Gamepad::RightShoulder(EventLoop* loop) const {
+  return BooleanEvent(loop,
+                      [this]() { return this->GetRightShoulderButton(); });
+}
+
+bool Gamepad::GetDpadUpButton() const {
+  return GetRawButton(Button::kDpadUp);
+}
+
+bool Gamepad::GetDpadUpButtonPressed() {
+  return GetRawButtonPressed(Button::kDpadUp);
+}
+
+bool Gamepad::GetDpadUpButtonReleased() {
+  return GetRawButtonReleased(Button::kDpadUp);
+}
+
+BooleanEvent Gamepad::DpadUp(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetDpadUpButton(); });
+}
+
+bool Gamepad::GetDpadDownButton() const {
+  return GetRawButton(Button::kDpadDown);
+}
+
+bool Gamepad::GetDpadDownButtonPressed() {
+  return GetRawButtonPressed(Button::kDpadDown);
+}
+
+bool Gamepad::GetDpadDownButtonReleased() {
+  return GetRawButtonReleased(Button::kDpadDown);
+}
+
+BooleanEvent Gamepad::DpadDown(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetDpadDownButton(); });
+}
+
+bool Gamepad::GetDpadLeftButton() const {
+  return GetRawButton(Button::kDpadLeft);
+}
+
+bool Gamepad::GetDpadLeftButtonPressed() {
+  return GetRawButtonPressed(Button::kDpadLeft);
+}
+
+bool Gamepad::GetDpadLeftButtonReleased() {
+  return GetRawButtonReleased(Button::kDpadLeft);
+}
+
+BooleanEvent Gamepad::DpadLeft(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetDpadLeftButton(); });
+}
+
+bool Gamepad::GetDpadRightButton() const {
+  return GetRawButton(Button::kDpadRight);
+}
+
+bool Gamepad::GetDpadRightButtonPressed() {
+  return GetRawButtonPressed(Button::kDpadRight);
+}
+
+bool Gamepad::GetDpadRightButtonReleased() {
+  return GetRawButtonReleased(Button::kDpadRight);
+}
+
+BooleanEvent Gamepad::DpadRight(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetDpadRightButton(); });
+}
+
+bool Gamepad::GetMisc1Button() const {
+  return GetRawButton(Button::kMisc1);
+}
+
+bool Gamepad::GetMisc1ButtonPressed() {
+  return GetRawButtonPressed(Button::kMisc1);
+}
+
+bool Gamepad::GetMisc1ButtonReleased() {
+  return GetRawButtonReleased(Button::kMisc1);
+}
+
+BooleanEvent Gamepad::Misc1(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetMisc1Button(); });
+}
+
+bool Gamepad::GetRightPaddle1Button() const {
+  return GetRawButton(Button::kRightPaddle1);
+}
+
+bool Gamepad::GetRightPaddle1ButtonPressed() {
+  return GetRawButtonPressed(Button::kRightPaddle1);
+}
+
+bool Gamepad::GetRightPaddle1ButtonReleased() {
+  return GetRawButtonReleased(Button::kRightPaddle1);
+}
+
+BooleanEvent Gamepad::RightPaddle1(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetRightPaddle1Button(); });
+}
+
+bool Gamepad::GetLeftPaddle1Button() const {
+  return GetRawButton(Button::kLeftPaddle1);
+}
+
+bool Gamepad::GetLeftPaddle1ButtonPressed() {
+  return GetRawButtonPressed(Button::kLeftPaddle1);
+}
+
+bool Gamepad::GetLeftPaddle1ButtonReleased() {
+  return GetRawButtonReleased(Button::kLeftPaddle1);
+}
+
+BooleanEvent Gamepad::LeftPaddle1(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetLeftPaddle1Button(); });
+}
+
+bool Gamepad::GetRightPaddle2Button() const {
+  return GetRawButton(Button::kRightPaddle2);
+}
+
+bool Gamepad::GetRightPaddle2ButtonPressed() {
+  return GetRawButtonPressed(Button::kRightPaddle2);
+}
+
+bool Gamepad::GetRightPaddle2ButtonReleased() {
+  return GetRawButtonReleased(Button::kRightPaddle2);
+}
+
+BooleanEvent Gamepad::RightPaddle2(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetRightPaddle2Button(); });
+}
+
+bool Gamepad::GetLeftPaddle2Button() const {
+  return GetRawButton(Button::kLeftPaddle2);
+}
+
+bool Gamepad::GetLeftPaddle2ButtonPressed() {
+  return GetRawButtonPressed(Button::kLeftPaddle2);
+}
+
+bool Gamepad::GetLeftPaddle2ButtonReleased() {
+  return GetRawButtonReleased(Button::kLeftPaddle2);
+}
+
+BooleanEvent Gamepad::LeftPaddle2(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetLeftPaddle2Button(); });
+}
+
+bool Gamepad::GetTouchpadButton() const {
+  return GetRawButton(Button::kTouchpad);
+}
+
+bool Gamepad::GetTouchpadButtonPressed() {
+  return GetRawButtonPressed(Button::kTouchpad);
+}
+
+bool Gamepad::GetTouchpadButtonReleased() {
+  return GetRawButtonReleased(Button::kTouchpad);
+}
+
+BooleanEvent Gamepad::Touchpad(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetTouchpadButton(); });
+}
+
+bool Gamepad::GetMisc2Button() const {
+  return GetRawButton(Button::kMisc2);
+}
+
+bool Gamepad::GetMisc2ButtonPressed() {
+  return GetRawButtonPressed(Button::kMisc2);
+}
+
+bool Gamepad::GetMisc2ButtonReleased() {
+  return GetRawButtonReleased(Button::kMisc2);
+}
+
+BooleanEvent Gamepad::Misc2(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetMisc2Button(); });
+}
+
+bool Gamepad::GetMisc3Button() const {
+  return GetRawButton(Button::kMisc3);
+}
+
+bool Gamepad::GetMisc3ButtonPressed() {
+  return GetRawButtonPressed(Button::kMisc3);
+}
+
+bool Gamepad::GetMisc3ButtonReleased() {
+  return GetRawButtonReleased(Button::kMisc3);
+}
+
+BooleanEvent Gamepad::Misc3(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetMisc3Button(); });
+}
+
+bool Gamepad::GetMisc4Button() const {
+  return GetRawButton(Button::kMisc4);
+}
+
+bool Gamepad::GetMisc4ButtonPressed() {
+  return GetRawButtonPressed(Button::kMisc4);
+}
+
+bool Gamepad::GetMisc4ButtonReleased() {
+  return GetRawButtonReleased(Button::kMisc4);
+}
+
+BooleanEvent Gamepad::Misc4(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetMisc4Button(); });
+}
+
+bool Gamepad::GetMisc5Button() const {
+  return GetRawButton(Button::kMisc5);
+}
+
+bool Gamepad::GetMisc5ButtonPressed() {
+  return GetRawButtonPressed(Button::kMisc5);
+}
+
+bool Gamepad::GetMisc5ButtonReleased() {
+  return GetRawButtonReleased(Button::kMisc5);
+}
+
+BooleanEvent Gamepad::Misc5(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetMisc5Button(); });
+}
+
+bool Gamepad::GetMisc6Button() const {
+  return GetRawButton(Button::kMisc6);
+}
+
+bool Gamepad::GetMisc6ButtonPressed() {
+  return GetRawButtonPressed(Button::kMisc6);
+}
+
+bool Gamepad::GetMisc6ButtonReleased() {
+  return GetRawButtonReleased(Button::kMisc6);
+}
+
+BooleanEvent Gamepad::Misc6(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetMisc6Button(); });
+}
+
+double Gamepad::GetAxisForSendable(int axis) const {
+  return DriverStation::GetStickAxisIfAvailable(GetPort(), axis).value_or(0.0);
+}
+
+bool Gamepad::GetButtonForSendable(int button) const {
+  return DriverStation::GetStickButtonIfAvailable(GetPort(), button)
+      .value_or(false);
+}
+
+void Gamepad::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("HID");
+  builder.PublishConstString("ControllerType", "Gamepad");
+  builder.AddDoubleProperty(
+      "LeftTrigger Axis",
+      [this] { return GetAxisForSendable(Axis::kLeftTrigger); }, nullptr);
+  builder.AddDoubleProperty(
+      "RightTrigger Axis",
+      [this] { return GetAxisForSendable(Axis::kRightTrigger); }, nullptr);
+  builder.AddDoubleProperty(
+      "LeftX", [this] { return GetAxisForSendable(Axis::kLeftX); }, nullptr);
+  builder.AddDoubleProperty(
+      "LeftY", [this] { return GetAxisForSendable(Axis::kLeftY); }, nullptr);
+  builder.AddDoubleProperty(
+      "RightX", [this] { return GetAxisForSendable(Axis::kRightX); }, nullptr);
+  builder.AddDoubleProperty(
+      "RightY", [this] { return GetAxisForSendable(Axis::kRightY); }, nullptr);
+  builder.AddBooleanProperty(
+      "SouthFace", [this] { return GetButtonForSendable(Button::kSouthFace); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "EastFace", [this] { return GetButtonForSendable(Button::kEastFace); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "WestFace", [this] { return GetButtonForSendable(Button::kWestFace); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "NorthFace", [this] { return GetButtonForSendable(Button::kNorthFace); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "Back", [this] { return GetButtonForSendable(Button::kBack); }, nullptr);
+  builder.AddBooleanProperty(
+      "Guide", [this] { return GetButtonForSendable(Button::kGuide); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "Start", [this] { return GetButtonForSendable(Button::kStart); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "LeftStick", [this] { return GetButtonForSendable(Button::kLeftStick); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "RightStick",
+      [this] { return GetButtonForSendable(Button::kRightStick); }, nullptr);
+  builder.AddBooleanProperty(
+      "LeftShoulder",
+      [this] { return GetButtonForSendable(Button::kLeftShoulder); }, nullptr);
+  builder.AddBooleanProperty(
+      "RightShoulder",
+      [this] { return GetButtonForSendable(Button::kRightShoulder); }, nullptr);
+  builder.AddBooleanProperty(
+      "DpadUp", [this] { return GetButtonForSendable(Button::kDpadUp); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "DpadDown", [this] { return GetButtonForSendable(Button::kDpadDown); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "DpadLeft", [this] { return GetButtonForSendable(Button::kDpadLeft); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "DpadRight", [this] { return GetButtonForSendable(Button::kDpadRight); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "Misc1", [this] { return GetButtonForSendable(Button::kMisc1); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "RightPaddle1",
+      [this] { return GetButtonForSendable(Button::kRightPaddle1); }, nullptr);
+  builder.AddBooleanProperty(
+      "LeftPaddle1",
+      [this] { return GetButtonForSendable(Button::kLeftPaddle1); }, nullptr);
+  builder.AddBooleanProperty(
+      "RightPaddle2",
+      [this] { return GetButtonForSendable(Button::kRightPaddle2); }, nullptr);
+  builder.AddBooleanProperty(
+      "LeftPaddle2",
+      [this] { return GetButtonForSendable(Button::kLeftPaddle2); }, nullptr);
+  builder.AddBooleanProperty(
+      "Touchpad", [this] { return GetButtonForSendable(Button::kTouchpad); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "Misc2", [this] { return GetButtonForSendable(Button::kMisc2); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "Misc3", [this] { return GetButtonForSendable(Button::kMisc3); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "Misc4", [this] { return GetButtonForSendable(Button::kMisc4); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "Misc5", [this] { return GetButtonForSendable(Button::kMisc5); },
+      nullptr);
+  builder.AddBooleanProperty(
+      "Misc6", [this] { return GetButtonForSendable(Button::kMisc6); },
+      nullptr);
+}

wpilibc/src/main/native/cpp/driverstation/GenericHID.cpp

@@ -0,0 +1,176 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/GenericHID.h"
+
+#include <string>
+
+#include <hal/DriverStation.h>
+
+#include "frc/DriverStation.h"
+#include "frc/Errors.h"
+#include "frc/event/BooleanEvent.h"
+
+using namespace frc;
+
+GenericHID::GenericHID(int port) {
+  if (port < 0 || port >= DriverStation::kJoystickPorts) {
+    throw FRC_MakeError(warn::BadJoystickIndex, "port {} out of range", port);
+  }
+  m_port = port;
+}
+
+bool GenericHID::GetRawButton(int button) const {
+  return DriverStation::GetStickButton(m_port, button);
+}
+
+bool GenericHID::GetRawButtonPressed(int button) {
+  return DriverStation::GetStickButtonPressed(m_port, button);
+}
+
+bool GenericHID::GetRawButtonReleased(int button) {
+  return DriverStation::GetStickButtonReleased(m_port, button);
+}
+
+BooleanEvent GenericHID::Button(int button, EventLoop* loop) const {
+  return BooleanEvent(loop,
+                      [this, button]() { return this->GetRawButton(button); });
+}
+
+double GenericHID::GetRawAxis(int axis) const {
+  return DriverStation::GetStickAxis(m_port, axis);
+}
+
+DriverStation::POVDirection GenericHID::GetPOV(int pov) const {
+  return DriverStation::GetStickPOV(m_port, pov);
+}
+
+BooleanEvent GenericHID::POV(DriverStation::POVDirection angle,
+                             EventLoop* loop) const {
+  return POV(0, angle, loop);
+}
+
+BooleanEvent GenericHID::POV(int pov, DriverStation::POVDirection angle,
+                             EventLoop* loop) const {
+  return BooleanEvent(
+      loop, [this, pov, angle] { return this->GetPOV(pov) == angle; });
+}
+
+BooleanEvent GenericHID::POVUp(EventLoop* loop) const {
+  return POV(DriverStation::kUp, loop);
+}
+
+BooleanEvent GenericHID::POVUpRight(EventLoop* loop) const {
+  return POV(DriverStation::kUpRight, loop);
+}
+
+BooleanEvent GenericHID::POVRight(EventLoop* loop) const {
+  return POV(DriverStation::kRight, loop);
+}
+
+BooleanEvent GenericHID::POVDownRight(EventLoop* loop) const {
+  return POV(DriverStation::kDownRight, loop);
+}
+
+BooleanEvent GenericHID::POVDown(EventLoop* loop) const {
+  return POV(DriverStation::kDown, loop);
+}
+
+BooleanEvent GenericHID::POVDownLeft(EventLoop* loop) const {
+  return POV(DriverStation::kDownLeft, loop);
+}
+
+BooleanEvent GenericHID::POVLeft(EventLoop* loop) const {
+  return POV(DriverStation::kLeft, loop);
+}
+
+BooleanEvent GenericHID::POVUpLeft(EventLoop* loop) const {
+  return POV(DriverStation::kUpLeft, loop);
+}
+
+BooleanEvent GenericHID::POVCenter(EventLoop* loop) const {
+  return POV(DriverStation::kCenter, loop);
+}
+
+BooleanEvent GenericHID::AxisLessThan(int axis, double threshold,
+                                      EventLoop* loop) const {
+  return BooleanEvent(loop, [this, axis, threshold]() {
+    return this->GetRawAxis(axis) < threshold;
+  });
+}
+
+BooleanEvent GenericHID::AxisGreaterThan(int axis, double threshold,
+                                         EventLoop* loop) const {
+  return BooleanEvent(loop, [this, axis, threshold]() {
+    return this->GetRawAxis(axis) > threshold;
+  });
+}
+
+int GenericHID::GetAxesMaximumIndex() const {
+  return DriverStation::GetStickAxesMaximumIndex(m_port);
+}
+
+int GenericHID::GetAxesAvailable() const {
+  return DriverStation::GetStickAxesAvailable(m_port);
+}
+
+int GenericHID::GetPOVsMaximumIndex() const {
+  return DriverStation::GetStickPOVsMaximumIndex(m_port);
+}
+
+int GenericHID::GetPOVsAvailable() const {
+  return DriverStation::GetStickPOVsAvailable(m_port);
+}
+
+int GenericHID::GetButtonsMaximumIndex() const {
+  return DriverStation::GetStickButtonsMaximumIndex(m_port);
+}
+
+uint64_t GenericHID::GetButtonsAvailable() const {
+  return DriverStation::GetStickButtonsAvailable(m_port);
+}
+
+bool GenericHID::IsConnected() const {
+  return DriverStation::IsJoystickConnected(m_port);
+}
+
+GenericHID::HIDType GenericHID::GetType() const {
+  return static_cast<HIDType>(DriverStation::GetJoystickType(m_port));
+}
+
+std::string GenericHID::GetName() const {
+  return DriverStation::GetJoystickName(m_port);
+}
+
+int GenericHID::GetPort() const {
+  return m_port;
+}
+
+void GenericHID::SetOutput(int outputNumber, bool value) {
+  m_outputs =
+      (m_outputs & ~(1 << (outputNumber - 1))) | (value << (outputNumber - 1));
+
+  HAL_SetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
+
+void GenericHID::SetOutputs(int value) {
+  m_outputs = value;
+  HAL_SetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
+
+void GenericHID::SetRumble(RumbleType type, double value) {
+  value = std::clamp(value, 0.0, 1.0);
+  double rumbleValue = value * 65535;
+
+  if (type == kLeftRumble) {
+    m_leftRumble = rumbleValue;
+  } else if (type == kRightRumble) {
+    m_rightRumble = rumbleValue;
+  } else {
+    m_leftRumble = rumbleValue;
+    m_rightRumble = rumbleValue;
+  }
+
+  HAL_SetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}

wpilibc/src/main/native/cpp/driverstation/Joystick.cpp

@@ -0,0 +1,131 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/Joystick.h"
+
+#include <cmath>
+
+#include <hal/UsageReporting.h>
+
+#include "frc/event/BooleanEvent.h"
+
+using namespace frc;
+
+Joystick::Joystick(int port) : GenericHID(port) {
+  m_axes[Axis::kX] = kDefaultXChannel;
+  m_axes[Axis::kY] = kDefaultYChannel;
+  m_axes[Axis::kZ] = kDefaultZChannel;
+  m_axes[Axis::kTwist] = kDefaultTwistChannel;
+  m_axes[Axis::kThrottle] = kDefaultThrottleChannel;
+
+  HAL_ReportUsage("HID", port, "Joystick");
+}
+
+void Joystick::SetXChannel(int channel) {
+  m_axes[Axis::kX] = channel;
+}
+
+void Joystick::SetYChannel(int channel) {
+  m_axes[Axis::kY] = channel;
+}
+
+void Joystick::SetZChannel(int channel) {
+  m_axes[Axis::kZ] = channel;
+}
+
+void Joystick::SetTwistChannel(int channel) {
+  m_axes[Axis::kTwist] = channel;
+}
+
+void Joystick::SetThrottleChannel(int channel) {
+  m_axes[Axis::kThrottle] = channel;
+}
+
+int Joystick::GetXChannel() const {
+  return m_axes[Axis::kX];
+}
+
+int Joystick::GetYChannel() const {
+  return m_axes[Axis::kY];
+}
+
+int Joystick::GetZChannel() const {
+  return m_axes[Axis::kZ];
+}
+
+int Joystick::GetTwistChannel() const {
+  return m_axes[Axis::kTwist];
+}
+
+int Joystick::GetThrottleChannel() const {
+  return m_axes[Axis::kThrottle];
+}
+
+double Joystick::GetX() const {
+  return GetRawAxis(m_axes[Axis::kX]);
+}
+
+double Joystick::GetY() const {
+  return GetRawAxis(m_axes[Axis::kY]);
+}
+
+double Joystick::GetZ() const {
+  return GetRawAxis(m_axes[Axis::kZ]);
+}
+
+double Joystick::GetTwist() const {
+  return GetRawAxis(m_axes[Axis::kTwist]);
+}
+
+double Joystick::GetThrottle() const {
+  return GetRawAxis(m_axes[Axis::kThrottle]);
+}
+
+bool Joystick::GetTrigger() const {
+  return GetRawButton(Button::kTrigger);
+}
+
+bool Joystick::GetTriggerPressed() {
+  return GetRawButtonPressed(Button::kTrigger);
+}
+
+bool Joystick::GetTriggerReleased() {
+  return GetRawButtonReleased(Button::kTrigger);
+}
+
+BooleanEvent Joystick::Trigger(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetTrigger(); });
+}
+
+bool Joystick::GetTop() const {
+  return GetRawButton(Button::kTop);
+}
+
+bool Joystick::GetTopPressed() {
+  return GetRawButtonPressed(Button::kTop);
+}
+
+bool Joystick::GetTopReleased() {
+  return GetRawButtonReleased(Button::kTop);
+}
+
+BooleanEvent Joystick::Top(EventLoop* loop) const {
+  return BooleanEvent(loop, [this]() { return this->GetTop(); });
+}
+
+double Joystick::GetMagnitude() const {
+  return std::hypot(GetX(), GetY());
+}
+
+units::radian_t Joystick::GetDirection() const {
+  // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html#joystick-and-controller-coordinate-system
+  // A positive rotation around the X axis moves the joystick right, and a
+  // positive rotation around the Y axis moves the joystick backward. When
+  // treating them as translations, 0 radians is measured from the right
+  // direction, and angle increases clockwise.
+  //
+  // It's rotated 90 degrees CCW (y is negated and the arguments are reversed)
+  // so that 0 radians is forward.
+  return units::radian_t{std::atan2(GetX(), -GetY())};
+}