From 55af553acc6ac08f1cd31ad2603003c4d2970189 Mon Sep 17 00:00:00 2001
From: Peter Johnson <johnson.peter@gmail.com>
Date: Fri, 11 Oct 2019 23:27:04 -0700
Subject: [PATCH] Simulation GUI: Map gamepad the same way as DS

---
 .../src/main/native/cpp/DriverStationGui.cpp  | 31 +++++++++++++++++--
 1 file changed, 28 insertions(+), 3 deletions(-)

diff --git a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
index 3de8732771..0189372609 100644
--- a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
@@ -211,7 +211,12 @@ void RobotJoystick::Update() {
   const unsigned char* sysButtons;
   if (sys->isGamepad && useGamepad) {
     sysAxes = sys->gamepadState.axes;
+    // don't remap on windows
+#ifdef _WIN32
+    sysButtons = sys->buttons;
+#else
     sysButtons = sys->gamepadState.buttons;
+#endif
   } else {
     sysAxes = sys->axes;
     sysButtons = sys->buttons;
@@ -226,13 +231,33 @@ void RobotJoystick::Update() {
   desc.buttonCount = (std::min)(sys->buttonCount, 32);
   desc.povCount = (std::min)(sys->hatCount, HAL_kMaxJoystickPOVs);
 
-  axes.count = desc.axisCount;
-  std::memcpy(axes.axes, sysAxes, axes.count * sizeof(&axes.axes[0]));
-
   buttons.count = desc.buttonCount;
   for (int j = 0; j < buttons.count; ++j)
     buttons.buttons |= (sysButtons[j] ? 1u : 0u) << j;
 
+  axes.count = desc.axisCount;
+  if (sys->isGamepad && useGamepad) {
+    // the FRC DriverStation maps gamepad (XInput) trigger values to 0-1 range
+    // on axis 2 and 3.
+    axes.axes[0] = sysAxes[0];
+    axes.axes[1] = sysAxes[1];
+    axes.axes[2] = 0.5 + sysAxes[4] / 2.0;
+    axes.axes[3] = 0.5 + sysAxes[5] / 2.0;
+    axes.axes[4] = sysAxes[2];
+    axes.axes[5] = sysAxes[3];
+
+    // the start button for gamepads is not mapped on the FRC DriverStation
+    // platforms, so remove it if present
+    if (buttons.count == 11) {
+      --desc.buttonCount;
+      --buttons.count;
+      buttons.buttons =
+          (buttons.buttons & 0xff) | ((buttons.buttons >> 1) & 0x300);
+    }
+  } else {
+    std::memcpy(axes.axes, sysAxes, axes.count * sizeof(&axes.axes[0]));
+  }
+
   povs.count = desc.povCount;
   for (int j = 0; j < povs.count; ++j) povs.povs[j] = HatToAngle(sys->hats[j]);
 }