[sim] Add joystick simulation support (#2595)

This adds joystick functions to DriverStationSim, and new GenericHIDSim,
JoystickSim, and XboxControllerSim classes.

wpilibc/src/main/native/include/frc/simulation/DriverStationSim.h

@@ -11,8 +11,10 @@
 #include <utility>
 
 #include <hal/simulation/DriverStationData.h>
+#include <hal/simulation/MockHooks.h>
 
 #include "CallbackStore.h"
+#include "frc/DriverStation.h"
 
 namespace frc {
 namespace sim {
@@ -108,7 +110,261 @@ class DriverStationSim {
     HALSIM_SetDriverStationDsAttached(dsAttached);
   }
 
-  static void NotifyNewData() { HALSIM_NotifyDriverStationNewData(); }
+  static std::unique_ptr<CallbackStore> RegisterAllianceStationIdCallback(
+      NotifyCallback callback, bool initialNotify) {
+    auto store = std::make_unique<CallbackStore>(
+        -1, callback, &HALSIM_CancelDriverStationAllianceStationIdCallback);
+    store->SetUid(HALSIM_RegisterDriverStationAllianceStationIdCallback(
+        &CallbackStoreThunk, store.get(), initialNotify));
+    return store;
+  }
+
+  static HAL_AllianceStationID GetAllianceStationId() {
+    return HALSIM_GetDriverStationAllianceStationId();
+  }
+
+  static void SetAllianceStationId(HAL_AllianceStationID allianceStationId) {
+    HALSIM_SetDriverStationAllianceStationId(allianceStationId);
+  }
+
+  static std::unique_ptr<CallbackStore> RegisterMatchTimeCallback(
+      NotifyCallback callback, bool initialNotify) {
+    auto store = std::make_unique<CallbackStore>(
+        -1, callback, &HALSIM_CancelDriverStationMatchTimeCallback);
+    store->SetUid(HALSIM_RegisterDriverStationMatchTimeCallback(
+        &CallbackStoreThunk, store.get(), initialNotify));
+    return store;
+  }
+
+  static double GetMatchTime() { return HALSIM_GetDriverStationMatchTime(); }
+
+  static void SetMatchTime(double matchTime) {
+    HALSIM_SetDriverStationMatchTime(matchTime);
+  }
+
+  /**
+   * Updates DriverStation data so that new values are visible to the user
+   * program.
+   */
+  static void NotifyNewData() {
+    HALSIM_NotifyDriverStationNewData();
+    DriverStation::GetInstance().WaitForData();
+  }
+
+  /**
+   * Sets suppression of DriverStation::ReportError and ReportWarning messages.
+   *
+   * @param shouldSend If false then messages will be suppressed.
+   */
+  static void SetSendError(bool shouldSend) {
+    if (shouldSend) {
+      HALSIM_SetSendError(nullptr);
+    } else {
+      HALSIM_SetSendError([](HAL_Bool isError, int32_t errorCode,
+                             HAL_Bool isLVCode, const char* details,
+                             const char* location, const char* callStack,
+                             HAL_Bool printMsg) { return 0; });
+    }
+  }
+
+  /**
+   * Sets suppression of DriverStation::SendConsoleLine messages.
+   *
+   * @param shouldSend If false then messages will be suppressed.
+   */
+  static void SetSendConsoleLine(bool shouldSend) {
+    if (shouldSend) {
+      HALSIM_SetSendConsoleLine(nullptr);
+    } else {
+      HALSIM_SetSendConsoleLine([](const char* line) { return 0; });
+    }
+  }
+
+  /**
+   * Gets the joystick outputs.
+   *
+   * @param stick The joystick number
+   * @return The joystick outputs
+   */
+  static int64_t GetJoystickOutputs(int stick) {
+    int64_t outputs = 0;
+    int32_t leftRumble;
+    int32_t rightRumble;
+    HALSIM_GetJoystickOutputs(stick, &outputs, &leftRumble, &rightRumble);
+    return outputs;
+  }
+
+  /**
+   * Gets the joystick rumble.
+   *
+   * @param stick The joystick number
+   * @param rumbleNum Rumble to get (0=left, 1=right)
+   * @return The joystick rumble value
+   */
+  static int GetJoystickRumble(int stick, int rumbleNum) {
+    int64_t outputs;
+    int32_t leftRumble = 0;
+    int32_t rightRumble = 0;
+    HALSIM_GetJoystickOutputs(stick, &outputs, &leftRumble, &rightRumble);
+    return rumbleNum == 0 ? leftRumble : rightRumble;
+  }
+
+  /**
+   * Sets the state of one joystick button. Button indexes begin at 1.
+   *
+   * @param stick The joystick number
+   * @param button The button index, beginning at 1
+   * @param state The state of the joystick button
+   */
+  static void SetJoystickButton(int stick, int button, bool state) {
+    HALSIM_SetJoystickButton(stick, button, state);
+  }
+
+  /**
+   * Gets the value of the axis on a joystick.
+   *
+   * @param stick The joystick number
+   * @param axis The analog axis number
+   * @param value The value of the axis on the joystick
+   */
+  static void SetJoystickAxis(int stick, int axis, double value) {
+    HALSIM_SetJoystickAxis(stick, axis, value);
+  }
+
+  /**
+   * Gets the state of a POV on a joystick.
+   *
+   * @param stick The joystick number
+   * @param pov The POV number
+   * @param value the angle of the POV in degrees, or -1 for not pressed
+   */
+  static void SetJoystickPOV(int stick, int pov, int value) {
+    HALSIM_SetJoystickPOV(stick, pov, value);
+  }
+
+  /**
+   * Sets the state of all the buttons on a joystick.
+   *
+   * @param stick The joystick number
+   * @param buttons The bitmap state of the buttons on the joystick
+   */
+  static void SetJoystickButtons(int stick, uint32_t buttons) {
+    HALSIM_SetJoystickButtonsValue(stick, buttons);
+  }
+
+  /**
+   * Sets the number of axes for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of axes on the indicated joystick
+   */
+  static void SetJoystickAxisCount(int stick, int count) {
+    HALSIM_SetJoystickAxisCount(stick, count);
+  }
+
+  /**
+   * Sets the number of POVs for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of POVs on the indicated joystick
+   */
+  static void SetJoystickPOVCount(int stick, int count) {
+    HALSIM_SetJoystickPOVCount(stick, count);
+  }
+
+  /**
+   * Sets the number of buttons for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of buttons on the indicated joystick
+   */
+  static void SetJoystickButtonCount(int stick, int count) {
+    HALSIM_SetJoystickButtonCount(stick, count);
+  }
+
+  /**
+   * Sets the value of isXbox for a joystick.
+   *
+   * @param stick The joystick number
+   * @param isXbox The value of isXbox
+   */
+  static void SetJoystickIsXbox(int stick, bool isXbox) {
+    HALSIM_SetJoystickIsXbox(stick, isXbox);
+  }
+
+  /**
+   * Sets the value of type for a joystick.
+   *
+   * @param stick The joystick number
+   * @param type The value of type
+   */
+  static void SetJoystickType(int stick, int type) {
+    HALSIM_SetJoystickType(stick, type);
+  }
+
+  /**
+   * Sets the name of a joystick.
+   *
+   * @param stick The joystick number
+   * @param name The value of name
+   */
+  static void SetJoystickName(int stick, const char* name) {
+    HALSIM_SetJoystickName(stick, name);
+  }
+
+  /**
+   * Sets the types of Axes for a joystick.
+   *
+   * @param stick The joystick number
+   * @param axis The target axis
+   * @param type The type of axis
+   */
+  static void SetJoystickAxisType(int stick, int axis, int type) {
+    HALSIM_SetJoystickAxisType(stick, axis, type);
+  }
+
+  /**
+   * Sets the game specific message.
+   *
+   * @param message the game specific message
+   */
+  static void SetGameSpecificMessage(const char* message) {
+    HALSIM_SetGameSpecificMessage(message);
+  }
+
+  /**
+   * Sets the event name.
+   *
+   * @param name the event name
+   */
+  static void SetEventName(const char* name) { HALSIM_SetEventName(name); }
+
+  /**
+   * Sets the match type.
+   *
+   * @param type the match type
+   */
+  static void SetMatchType(DriverStation::MatchType type) {
+    HALSIM_SetMatchType(static_cast<HAL_MatchType>(static_cast<int>(type)));
+  }
+
+  /**
+   * Sets the match number.
+   *
+   * @param matchNumber the match number
+   */
+  static void SetMatchNumber(int matchNumber) {
+    HALSIM_SetMatchNumber(matchNumber);
+  }
+
+  /**
+   * Sets the replay number.
+   *
+   * @param replayNumber the replay number
+   */
+  static void SetReplayNumber(int replayNumber) {
+    HALSIM_SetReplayNumber(replayNumber);
+  }
 
   static void ResetData() { HALSIM_ResetDriverStationData(); }
 };

wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h

@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/GenericHID.h"
+#include "frc/simulation/DriverStationSim.h"
+
+namespace frc {
+namespace sim {
+
+/**
+ * Class to control a simulated generic joystick.
+ */
+class GenericHIDSim {
+ public:
+  /**
+   * Constructs from a GenericHID object.
+   *
+   * @param joystick joystick to simulate
+   */
+  explicit GenericHIDSim(const GenericHID& joystick)
+      : m_port{joystick.GetPort()} {}
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  explicit GenericHIDSim(int port) : m_port{port} {}
+
+  /**
+   * Updates joystick data so that new values are visible to the user program.
+   */
+  void NotifyNewData() { DriverStationSim::NotifyNewData(); }
+
+  void SetRawButton(int button, bool value) {
+    DriverStationSim::SetJoystickButton(m_port, button, value);
+  }
+
+  void SetRawAxis(int axis, double value) {
+    DriverStationSim::SetJoystickAxis(m_port, axis, value);
+  }
+
+  void SetPOV(int pov, int value) {
+    DriverStationSim::SetJoystickPOV(m_port, pov, value);
+  }
+
+  void SetPOV(int value) { SetPOV(0, value); }
+
+  void SetAxisCount(int count) {
+    DriverStationSim::SetJoystickAxisCount(m_port, count);
+  }
+
+  void SetPOVCount(int count) {
+    DriverStationSim::SetJoystickPOVCount(m_port, count);
+  }
+
+  void SetButtonCount(int count) {
+    DriverStationSim::SetJoystickButtonCount(m_port, count);
+  }
+
+  void SetType(GenericHID::HIDType type) {
+    DriverStationSim::SetJoystickType(m_port, type);
+  }
+
+  void SetName(const char* name) {
+    DriverStationSim::SetJoystickName(m_port, name);
+  }
+
+  void SetAxisType(int axis, int type) {
+    DriverStationSim::SetJoystickAxisType(m_port, axis, type);
+  }
+
+  bool GetOutput(int outputNumber) {
+    int64_t outputs = GetOutputs();
+    return (outputs & (static_cast<int64_t>(1) << (outputNumber - 1))) != 0;
+  }
+
+  int64_t GetOutputs() { return DriverStationSim::GetJoystickOutputs(m_port); }
+
+  double GetRumble(GenericHID::RumbleType type) {
+    int value = DriverStationSim::GetJoystickRumble(
+        m_port, type == GenericHID::kLeftRumble ? 0 : 1);
+    return value / 65535.0;
+  }
+
+ protected:
+  int m_port;
+};
+
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/frc/simulation/JoystickSim.h

@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/Joystick.h"
+#include "frc/simulation/GenericHIDSim.h"
+
+namespace frc {
+namespace sim {
+
+/**
+ * Class to control a simulated joystick.
+ */
+class JoystickSim : public GenericHIDSim {
+ public:
+  /**
+   * Constructs from a Joystick object.
+   *
+   * @param joystick joystick to simulate
+   */
+  explicit JoystickSim(const Joystick& joystick)
+      : GenericHIDSim{joystick}, m_joystick{&joystick} {
+    // default to a reasonable joystick configuration
+    SetAxisCount(5);
+    SetButtonCount(12);
+    SetPOVCount(1);
+  }
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  explicit JoystickSim(int port) : GenericHIDSim{port} {
+    // default to a reasonable joystick configuration
+    SetAxisCount(5);
+    SetButtonCount(12);
+    SetPOVCount(1);
+  }
+
+  void SetX(double value) {
+    SetRawAxis(
+        m_joystick ? m_joystick->GetXChannel() : Joystick::kDefaultXChannel,
+        value);
+  }
+
+  void SetY(double value) {
+    SetRawAxis(
+        m_joystick ? m_joystick->GetYChannel() : Joystick::kDefaultYChannel,
+        value);
+  }
+
+  void SetZ(double value) {
+    SetRawAxis(
+        m_joystick ? m_joystick->GetZChannel() : Joystick::kDefaultZChannel,
+        value);
+  }
+
+  void SetTwist(double value) {
+    SetRawAxis(m_joystick ? m_joystick->GetTwistChannel()
+                          : Joystick::kDefaultTwistChannel,
+               value);
+  }
+
+  void SetThrottle(double value) {
+    SetRawAxis(m_joystick ? m_joystick->GetThrottleChannel()
+                          : Joystick::kDefaultThrottleChannel,
+               value);
+  }
+
+  void SetTrigger(bool state) { SetRawButton(1, state); }
+
+  void SetTop(bool state) { SetRawButton(2, state); }
+
+ private:
+  const Joystick* m_joystick = nullptr;
+};
+
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/frc/simulation/XboxControllerSim.h

@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/XboxController.h"
+#include "frc/simulation/GenericHIDSim.h"
+
+namespace frc {
+namespace sim {
+
+/**
+ * Class to control a simulated Xbox 360 or Xbox One controller.
+ */
+class XboxControllerSim : public GenericHIDSim {
+ public:
+  /**
+   * Constructs from a XboxController object.
+   *
+   * @param joystick controller to simulate
+   */
+  explicit XboxControllerSim(const XboxController& joystick)
+      : GenericHIDSim{joystick} {
+    SetAxisCount(6);
+    SetButtonCount(10);
+  }
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  explicit XboxControllerSim(int port) : GenericHIDSim{port} {
+    SetAxisCount(6);
+    SetButtonCount(10);
+  }
+
+  void SetX(GenericHID::JoystickHand hand, double value) {
+    if (hand == GenericHID::kLeftHand) {
+      SetRawAxis(static_cast<int>(XboxController::Axis::kLeftX), value);
+    } else {
+      SetRawAxis(static_cast<int>(XboxController::Axis::kRightX), value);
+    }
+  }
+
+  void SetY(GenericHID::JoystickHand hand, double value) {
+    if (hand == GenericHID::kLeftHand) {
+      SetRawAxis(static_cast<int>(XboxController::Axis::kLeftY), value);
+    } else {
+      SetRawAxis(static_cast<int>(XboxController::Axis::kRightY), value);
+    }
+  }
+
+  void SetTriggerAxis(GenericHID::JoystickHand hand, double value) {
+    if (hand == GenericHID::kLeftHand) {
+      SetRawAxis(static_cast<int>(XboxController::Axis::kLeftTrigger), value);
+    } else {
+      SetRawAxis(static_cast<int>(XboxController::Axis::kRightTrigger), value);
+    }
+  }
+
+  void SetBumper(GenericHID::JoystickHand hand, bool state) {
+    if (hand == GenericHID::kLeftHand) {
+      SetRawButton(static_cast<int>(XboxController::Button::kBumperLeft),
+                   state);
+    } else {
+      SetRawButton(static_cast<int>(XboxController::Button::kBumperRight),
+                   state);
+    }
+  }
+
+  void SetStickButton(GenericHID::JoystickHand hand, bool state) {
+    if (hand == GenericHID::kLeftHand) {
+      SetRawButton(static_cast<int>(XboxController::Button::kStickLeft), state);
+    } else {
+      SetRawButton(static_cast<int>(XboxController::Button::kStickRight),
+                   state);
+    }
+  }
+
+  void SetAButton(bool state) {
+    SetRawButton(static_cast<int>(XboxController::Button::kA), state);
+  }
+
+  void SetBButton(bool state) {
+    SetRawButton(static_cast<int>(XboxController::Button::kB), state);
+  }
+
+  void SetXButton(bool state) {
+    SetRawButton(static_cast<int>(XboxController::Button::kX), state);
+  }
+
+  void SetYButton(bool state) {
+    SetRawButton(static_cast<int>(XboxController::Button::kY), state);
+  }
+
+  void SetBackButton(bool state) {
+    SetRawButton(static_cast<int>(XboxController::Button::kBack), state);
+  }
+
+  void SetStartButton(bool state) {
+    SetRawButton(static_cast<int>(XboxController::Button::kStart), state);
+  }
+};
+
+}  // namespace sim
+}  // namespace frc