[hal] Fix potential gamedata out of bounds read (#3983)

The size was uninitialized.  If the size is smaller than the data,
NetComm just updates the size and does not initialize the data.

glass/publish.gradle

@@ -130,6 +130,14 @@ model {
                             }
 
                             from(applicationPath)
+
+                            if (binary.targetPlatform.operatingSystem.isWindows()) {
+                                def exePath = binary.executable.file.absolutePath
+                                exePath = exePath.substring(0, exePath.length() - 4)
+                                def pdbPath = new File(exePath + '.pdb')
+                                from(pdbPath)
+                            }
+
                             into(nativeUtils.getPlatformPath(binary))
                         }
 

glass/src/libnt/native/include/glass/networktables/NTSpeedController.h

@@ -16,7 +16,7 @@
 namespace glass {
 class NTSpeedControllerModel : public SpeedControllerModel {
  public:
-  static constexpr const char* kType = "Speed Controller";
+  static constexpr const char* kType = "Motor Controller";
 
   explicit NTSpeedControllerModel(std::string_view path);
   NTSpeedControllerModel(NT_Inst instance, std::string_view path);

hal/src/main/native/athena/CTREPCM.cpp

@@ -335,8 +335,13 @@ HAL_Bool HAL_GetCTREPCMSolenoidVoltageFault(HAL_CTREPCMHandle handle,
 
 void HAL_ClearAllCTREPCMStickyFaults(HAL_CTREPCMHandle handle,
                                      int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
   uint8_t controlData[] = {0, 0, 0, 0x80};
-  HAL_WriteCANPacket(handle, controlData, sizeof(controlData), Control2,
+  HAL_WriteCANPacket(pcm->canHandle, controlData, sizeof(controlData), Control2,
                      status);
 }
 
@@ -393,7 +398,7 @@ void HAL_SetCTREPCMOneShotDuration(HAL_CTREPCMHandle handle, int32_t index,
       (std::min)(static_cast<uint32_t>(durMs) / 10,
                  static_cast<uint32_t>(0xFF));
   HAL_WriteCANPacketRepeating(pcm->canHandle, pcm->oneShot.sol10MsPerUnit, 8,
-                              Control2, SendPeriod, status);
+                              Control3, SendPeriod, status);
 }
 
 }  // extern "C"

hal/src/main/native/athena/FRCDriverStation.cpp

@@ -110,10 +110,15 @@ static int32_t HAL_GetControlWordInternal(HAL_ControlWord* controlWord) {
 
 static int32_t HAL_GetMatchInfoInternal(HAL_MatchInfo* info) {
   MatchType_t matchType = MatchType_t::kMatchType_none;
+  info->gameSpecificMessageSize = sizeof(info->gameSpecificMessage);
   int status = FRC_NetworkCommunication_getMatchInfo(
       info->eventName, &matchType, &info->matchNumber, &info->replayNumber,
       info->gameSpecificMessage, &info->gameSpecificMessageSize);
 
+  if (info->gameSpecificMessageSize > sizeof(info->gameSpecificMessage)) {
+    info->gameSpecificMessageSize = 0;
+  }
+
   info->matchType = static_cast<HAL_MatchType>(matchType);
 
   *(std::end(info->eventName) - 1) = '\0';

myRobot/build.gradle

@@ -86,6 +86,9 @@ deploy {
 
                 myRobotCpp(NativeExecutableArtifact) {
                     libraryDirectory = '/usr/local/frc/third-party/lib'
+                    def excludes = getLibraryFilter().getExcludes()
+                    excludes.add('**/*.so.debug')
+                    excludes.add('**/*.so.*.debug')
                     postdeploy << { ctx ->
                         ctx.execute('chmod +x myRobotCpp')
                     }

outlineviewer/publish.gradle

@@ -76,6 +76,14 @@ model {
                             }
 
                             from(applicationPath)
+
+                            if (binary.targetPlatform.operatingSystem.isWindows()) {
+                                def exePath = binary.executable.file.absolutePath
+                                exePath = exePath.substring(0, exePath.length() - 4)
+                                def pdbPath = new File(exePath + '.pdb')
+                                from(pdbPath)
+                            }
+
                             into(nativeUtils.getPlatformPath(binary))
                         }
 

roborioteamnumbersetter/CMakeLists.txt

@@ -12,7 +12,7 @@ file(GLOB rtns_src src/main/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibV
 if (WIN32)
     set(rtns_rc src/main/native/win/roborioteamnumbersetter.rc)
 elseif(APPLE)
-    set(MACOSX_BUNDLE_ICON_FILE glass.icns)
+    set(MACOSX_BUNDLE_ICON_FILE rtns.icns)
     set(APP_ICON_MACOSX src/main/native/mac/rtns.icns)
     set_source_files_properties(${APP_ICON_MACOSX} PROPERTIES MACOSX_PACKAGE_LOCATION "Resources")
 endif()

roborioteamnumbersetter/publish.gradle

@@ -76,6 +76,14 @@ model {
                             }
 
                             from(applicationPath)
+
+                            if (binary.targetPlatform.operatingSystem.isWindows()) {
+                                def exePath = binary.executable.file.absolutePath
+                                exePath = exePath.substring(0, exePath.length() - 4)
+                                def pdbPath = new File(exePath + '.pdb')
+                                from(pdbPath)
+                            }
+
                             into(nativeUtils.getPlatformPath(binary))
                         }
 

roborioteamnumbersetter/src/main/native/cpp/App.cpp

@@ -31,6 +31,16 @@ namespace gui = wpi::gui;
 
 const char* GetWPILibVersion();
 
+namespace rtns {
+std::string_view GetResource_rtns_16_png();
+std::string_view GetResource_rtns_32_png();
+std::string_view GetResource_rtns_48_png();
+std::string_view GetResource_rtns_64_png();
+std::string_view GetResource_rtns_128_png();
+std::string_view GetResource_rtns_256_png();
+std::string_view GetResource_rtns_512_png();
+}  // namespace rtns
+
 #define GLFWAPI extern "C"
 GLFWAPI void glfwGetWindowSize(GLFWwindow* window, int* width, int* height);
 #define GLFW_DONT_CARE -1
@@ -240,6 +250,15 @@ void Application(std::string_view saveDir) {
   gui::CreateContext();
   glass::CreateContext();
 
+  // Add icons
+  gui::AddIcon(rtns::GetResource_rtns_16_png());
+  gui::AddIcon(rtns::GetResource_rtns_32_png());
+  gui::AddIcon(rtns::GetResource_rtns_48_png());
+  gui::AddIcon(rtns::GetResource_rtns_64_png());
+  gui::AddIcon(rtns::GetResource_rtns_128_png());
+  gui::AddIcon(rtns::GetResource_rtns_256_png());
+  gui::AddIcon(rtns::GetResource_rtns_512_png());
+
   glass::SetStorageName("roborioteamnumbersetter");
   glass::SetStorageDir(saveDir.empty() ? gui::GetPlatformSaveFileDir()
                                        : saveDir);

shared/config.gradle

@@ -10,7 +10,7 @@ nativeUtils {
     wpi {
         configureDependencies {
             wpiVersion = "-1"
-            niLibVersion = "2022.2.3"
+            niLibVersion = "2022.4.0"
             opencvVersion = "4.5.2-1"
             googleTestVersion = "1.9.0-5-437e100-1"
             imguiVersion = "1.86-1"
@@ -21,7 +21,10 @@ nativeUtils {
 
 nativeUtils.wpi.addWarnings()
 nativeUtils.wpi.addWarningsAsErrors()
-nativeUtils.wpi.addReleaseSymbolGeneration()
+
+if (project.name != 'wpilibcExamples') {
+    nativeUtils.wpi.addReleaseSymbolGeneration()
+}
 
 nativeUtils.setSinglePrintPerPlatform()
 nativeUtils.enableSourceLink()

wpilibNewCommands/WPILibNewCommands.json

@@ -1,7 +1,7 @@
 {
   "fileName": "WPILibNewCommands.json",
   "name": "WPILib-New-Commands",
-  "version": "2020.0.0",
+  "version": "1.0.0",
   "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
   "mavenUrls": [],
   "jsonUrl": "",

wpilibOldCommands/WPILibOldCommands.json

@@ -1,7 +1,7 @@
 {
   "fileName": "WPILibOldCommands.json",
   "name": "WPILib-Old-Commands",
-  "version": "2020.0.0",
+  "version": "1.0.0",
   "uuid": "b066afc2-5c18-43c4-b758-43381fcb275e",
   "mavenUrls": [],
   "jsonUrl": "",

wpilibc/src/main/native/cpp/PneumaticHub.cpp

@@ -4,6 +4,7 @@
 
 #include "frc/PneumaticHub.h"
 
+#include <fmt/format.h>
 #include <hal/REVPH.h>
 #include <wpi/NullDeleter.h>
 #include <wpi/StackTrace.h>
@@ -11,6 +12,7 @@
 #include "frc/Compressor.h"
 #include "frc/DoubleSolenoid.h"
 #include "frc/Errors.h"
+#include "frc/RobotBase.h"
 #include "frc/SensorUtil.h"
 #include "frc/Solenoid.h"
 #include "frc/fmt/Units.h"
@@ -56,6 +58,29 @@ class PneumaticHub::DataStore {
         std::shared_ptr<DataStore>{this, wpi::NullDeleter<DataStore>()};
 
     auto version = m_moduleObject.GetVersion();
+
+    if (version.FirmwareMajor > 0 && RobotBase::IsReal()) {
+      // Write PH firmware version to roboRIO
+      std::FILE* file = nullptr;
+      file = std::fopen(
+          fmt::format("/tmp/frc_versions/REV_PH_{:0>2}_WPILib_Version.ini",
+                      module)
+              .c_str(),
+          "w");
+      if (file != nullptr) {
+        std::fputs("[Version]\n", file);
+        std::fputs(fmt::format("model=REV PH\n").c_str(), file);
+        std::fputs(fmt::format("deviceID={:x}\n", (0x9052600 | module)).c_str(),
+                   file);
+        std::fputs(fmt::format("currentVersion={}.{}.{}", version.FirmwareMajor,
+                               version.FirmwareMinor, version.FirmwareFix)
+                       .c_str(),
+                   file);
+        std::fclose(file);
+      }
+    }
+
+    // Check PH firmware version
     if (version.FirmwareMajor > 0 && version.FirmwareMajor < 22) {
       throw FRC_MakeError(
           err::AssertionFailure,
@@ -76,7 +101,7 @@ class PneumaticHub::DataStore {
   bool m_compressorReserved{false};
   wpi::mutex m_reservedLock;
   PneumaticHub m_moduleObject{HAL_kInvalidHandle, 0};
-  std::array<units::second_t, 16> m_oneShotDurMs{0_s};
+  std::array<units::millisecond_t, 16> m_oneShotDurMs{0_ms};
 };
 
 PneumaticHub::PneumaticHub()

wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp

@@ -80,20 +80,20 @@ PneumaticsControlModule::PneumaticsControlModule(HAL_CTREPCMHandle handle,
 bool PneumaticsControlModule::GetCompressor() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMCompressor(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 
 void PneumaticsControlModule::DisableCompressor() {
   int32_t status = 0;
   HAL_SetCTREPCMClosedLoopControl(m_handle, false, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
 }
 
 void PneumaticsControlModule::EnableCompressorDigital() {
   int32_t status = 0;
   HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
 }
 
 void PneumaticsControlModule::EnableCompressorAnalog(
@@ -101,7 +101,7 @@ void PneumaticsControlModule::EnableCompressorAnalog(
     units::pounds_per_square_inch_t maxPressure) {
   int32_t status = 0;
   HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
 }
 
 void PneumaticsControlModule::EnableCompressorHybrid(
@@ -109,13 +109,13 @@ void PneumaticsControlModule::EnableCompressorHybrid(
     units::pounds_per_square_inch_t maxPressure) {
   int32_t status = 0;
   HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
 }
 
 CompressorConfigType PneumaticsControlModule::GetCompressorConfigType() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMClosedLoopControl(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result ? CompressorConfigType::Digital
                 : CompressorConfigType::Disabled;
 }
@@ -123,85 +123,85 @@ CompressorConfigType PneumaticsControlModule::GetCompressorConfigType() const {
 bool PneumaticsControlModule::GetPressureSwitch() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMPressureSwitch(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 
 units::ampere_t PneumaticsControlModule::GetCompressorCurrent() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMCompressorCurrent(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return units::ampere_t{result};
 }
 
 bool PneumaticsControlModule::GetCompressorCurrentTooHighFault() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMCompressorCurrentTooHighFault(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 bool PneumaticsControlModule::GetCompressorCurrentTooHighStickyFault() const {
   int32_t status = 0;
   auto result =
       HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 bool PneumaticsControlModule::GetCompressorShortedFault() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMCompressorShortedFault(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 bool PneumaticsControlModule::GetCompressorShortedStickyFault() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMCompressorShortedStickyFault(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 bool PneumaticsControlModule::GetCompressorNotConnectedFault() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMCompressorNotConnectedFault(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 bool PneumaticsControlModule::GetCompressorNotConnectedStickyFault() const {
   int32_t status = 0;
   auto result =
       HAL_GetCTREPCMCompressorNotConnectedStickyFault(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 
 bool PneumaticsControlModule::GetSolenoidVoltageFault() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMSolenoidVoltageFault(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 bool PneumaticsControlModule::GetSolenoidVoltageStickyFault() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMSolenoidVoltageStickyFault(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 
 void PneumaticsControlModule::ClearAllStickyFaults() {
   int32_t status = 0;
   HAL_ClearAllCTREPCMStickyFaults(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
 }
 
 void PneumaticsControlModule::SetSolenoids(int mask, int values) {
   int32_t status = 0;
   HAL_SetCTREPCMSolenoids(m_handle, mask, values, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
 }
 
 int PneumaticsControlModule::GetSolenoids() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMSolenoids(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 
@@ -212,14 +212,14 @@ int PneumaticsControlModule::GetModuleNumber() const {
 int PneumaticsControlModule::GetSolenoidDisabledList() const {
   int32_t status = 0;
   auto result = HAL_GetCTREPCMSolenoidDisabledList(m_handle, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
   return result;
 }
 
 void PneumaticsControlModule::FireOneShot(int index) {
   int32_t status = 0;
   HAL_FireCTREPCMOneShot(m_handle, index, &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
 }
 
 void PneumaticsControlModule::SetOneShotDuration(int index,
@@ -227,7 +227,7 @@ void PneumaticsControlModule::SetOneShotDuration(int index,
   int32_t status = 0;
   units::millisecond_t millis = duration;
   HAL_SetCTREPCMOneShotDuration(m_handle, index, millis.to<int32_t>(), &status);
-  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  FRC_ReportError(status, "Module {}", m_module);
 }
 
 bool PneumaticsControlModule::CheckSolenoidChannel(int channel) const {

wpilibc/src/main/native/cpp/PowerDistribution.cpp

@@ -124,6 +124,13 @@ int PowerDistribution::GetModule() const {
   return m_module;
 }
 
+PowerDistribution::ModuleType PowerDistribution::GetType() const {
+  int32_t status = 0;
+  auto type = HAL_GetPowerDistributionType(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return static_cast<ModuleType>(type);
+}
+
 bool PowerDistribution::GetSwitchableChannel() const {
   int32_t status = 0;
   bool state = HAL_GetPowerDistributionSwitchableChannel(m_handle, &status);

wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp

@@ -31,6 +31,7 @@ static constexpr const char* widgetStrings[] = {
     "Differential Drivebase",
     "Mecanum Drivebase",
     "Camera Stream",
+    "Field",
 };
 
 const char* detail::GetStringForWidgetType(BuiltInWidgets type) {

wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp

@@ -0,0 +1,46 @@
+// 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/simulation/DCMotorSim.h"
+
+#include <wpi/MathExtras.h>
+
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DCMotorSim::DCMotorSim(const LinearSystem<2, 1, 2>& plant,
+                       const DCMotor& gearbox, double gearing,
+                       const std::array<double, 2>& measurementStdDevs)
+    : LinearSystemSim<2, 1, 2>(plant, measurementStdDevs),
+      m_gearbox(gearbox),
+      m_gearing(gearing) {}
+
+DCMotorSim::DCMotorSim(const DCMotor& gearbox, double gearing,
+                       units::kilogram_square_meter_t moi,
+                       const std::array<double, 2>& measurementStdDevs)
+    : DCMotorSim(LinearSystemId::DCMotorSystem(gearbox, moi, gearing), gearbox,
+                 gearing, measurementStdDevs) {}
+
+units::radian_t DCMotorSim::GetAngularPosition() const {
+  return units::radian_t{GetOutput(0)};
+}
+
+units::radians_per_second_t DCMotorSim::GetAngularVelocity() const {
+  return units::radians_per_second_t{GetOutput(1)};
+}
+
+units::ampere_t DCMotorSim::GetCurrentDraw() const {
+  // I = V / R - omega / (Kv * R)
+  // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
+  // is spinning 10x faster than the output.
+  return m_gearbox.Current(GetAngularVelocity() * m_gearing,
+                           units::volt_t{m_u(0)}) *
+         wpi::sgn(m_u(0));
+}
+
+void DCMotorSim::SetInputVoltage(units::volt_t voltage) {
+  SetInput(Eigen::Vector<double, 1>{voltage.value()});
+}

wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp

@@ -49,7 +49,20 @@ void MechanismLigament2d::SetLineWeight(double lineWidth) {
   Flush();
 }
 
+Color8Bit MechanismLigament2d::GetColor() {
+  std::scoped_lock lock(m_mutex);
+  if (m_colorEntry) {
+    auto color = m_colorEntry.GetString("");
+    std::strncpy(m_color, color.c_str(), sizeof(m_color));
+    m_color[sizeof(m_color) - 1] = '\0';
+  }
+  unsigned int r = 0, g = 0, b = 0;
+  std::sscanf(m_color, "#%02X%02X%02X", &r, &g, &b);
+  return {static_cast<int>(r), static_cast<int>(g), static_cast<int>(b)};
+}
+
 double MechanismLigament2d::GetAngle() {
+  std::scoped_lock lock(m_mutex);
   if (m_angleEntry) {
     m_angle = m_angleEntry.GetDouble(0.0);
   }
@@ -57,12 +70,21 @@ double MechanismLigament2d::GetAngle() {
 }
 
 double MechanismLigament2d::GetLength() {
+  std::scoped_lock lock(m_mutex);
   if (m_lengthEntry) {
     m_length = m_lengthEntry.GetDouble(0.0);
   }
   return m_length;
 }
 
+double MechanismLigament2d::GetLineWeight() {
+  std::scoped_lock lock(m_mutex);
+  if (m_weightEntry) {
+    m_weight = m_weightEntry.GetDouble(0.0);
+  }
+  return m_weight;
+}
+
 void MechanismLigament2d::SetLength(double length) {
   std::scoped_lock lock(m_mutex);
   m_length = length;

wpilibc/src/main/native/include/frc/PowerDistribution.h

@@ -12,7 +12,7 @@ namespace frc {
 
 /**
  * Class for getting voltage, current, temperature, power and energy from the
- * CAN PDP.
+ * CTRE Power Distribution Panel (PDP) or REV Power Distribution Hub (PDH).
  */
 class PowerDistribution : public wpi::Sendable,
                           public wpi::SendableHelper<PowerDistribution> {
@@ -21,16 +21,17 @@ class PowerDistribution : public wpi::Sendable,
   enum class ModuleType { kCTRE = 1, kRev = 2 };
 
   /**
-   * Constructs a PowerDistribution.
+   * Constructs a PowerDistribution object.
    *
-   * Uses the default CAN ID (0 for CTRE and 1 for REV).
+   * Detects the connected PDP/PDH using the default CAN ID (0 for CTRE and 1
+   * for REV).
    */
   PowerDistribution();
 
   /**
-   * Constructs a PowerDistribution.
+   * Constructs a PowerDistribution object.
    *
-   * @param module The CAN ID of the PDP
+   * @param module The CAN ID of the PDP/PDH
    * @param moduleType The type of module
    */
   PowerDistribution(int module, ModuleType moduleType);
@@ -40,56 +41,57 @@ class PowerDistribution : public wpi::Sendable,
   PowerDistribution& operator=(PowerDistribution&&) = default;
 
   /**
-   * Query the input voltage of the PDP.
+   * Query the input voltage of the PDP/PDH.
    *
-   * @return The voltage of the PDP in volts
+   * @return The input voltage in volts
    */
   double GetVoltage() const;
 
   /**
-   * Query the temperature of the PDP.
+   * Query the temperature of the PDP/PDH.
    *
-   * @return The temperature of the PDP in degrees Celsius
+   * @return The temperature in degrees Celsius
    */
   double GetTemperature() const;
 
   /**
-   * Query the current of a single channel of the PDP.
+   * Query the current of a single channel of the PDP/PDH.
    *
-   * @return The current of one of the PDP channels (channels 0-15) in Amperes
+   * @param channel the channel to query (0-15 for PDP, 0-23 for PDH)
+   * @return The current of the channel in Amperes
    */
   double GetCurrent(int channel) const;
 
   /**
-   * Query the total current of all monitored PDP channels (0-15).
+   * Query the total current of all monitored PDP/PDH channels.
    *
-   * @return The the total current drawn from the PDP channels in Amperes
+   * @return The total current drawn from all channels in Amperes
    */
   double GetTotalCurrent() const;
 
   /**
-   * Query the total power drawn from the monitored PDP channels.
+   * Query the total power drawn from all monitored PDP/PDH channels.
    *
-   * @return The the total power drawn from the PDP channels in Watts
+   * @return The total power drawn in Watts
    */
   double GetTotalPower() const;
 
   /**
-   * Query the total energy drawn from the monitored PDP channels.
+   * Query the total energy drawn from the monitored PDP/PDH channels.
    *
-   * @return The the total energy drawn from the PDP channels in Joules
+   * @return The total energy drawn in Joules
    */
   double GetTotalEnergy() const;
 
   /**
-   * Reset the total energy drawn from the PDP.
+   * Reset the total energy drawn from the PDP/PDH.
    *
    * @see PowerDistribution#GetTotalEnergy
    */
   void ResetTotalEnergy();
 
   /**
-   * Remove all of the fault flags on the PDP.
+   * Remove all of the fault flags on the PDP/PDH.
    */
   void ClearStickyFaults();
 
@@ -98,16 +100,32 @@ class PowerDistribution : public wpi::Sendable,
    */
   int GetModule() const;
 
+  /**
+   * Gets module type.
+   */
+  ModuleType GetType() const;
+
+  /**
+   * Gets whether the PDH switchable channel is turned on or off. Returns false
+   * with the CTRE PDP.
+   *
+   * @return The output state of the PDH switchable channel
+   */
   bool GetSwitchableChannel() const;
 
+  /**
+   * Sets the PDH switchable channel on or off. Does nothing with the CTRE PDP.
+   *
+   * @param enabled Whether to turn the PDH switchable channel on or off
+   */
   void SetSwitchableChannel(bool enabled);
 
   struct Version {
     uint32_t FirmwareMajor;
     uint32_t FirmwareMinor;
     uint32_t FirmwareFix;
-    uint32_t FardwareMinor;
-    uint32_t FardwareMajor;
+    uint32_t HardwareMinor;
+    uint32_t HardwareMajor;
     uint32_t UniqueId;
   };
 

wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h

@@ -101,6 +101,11 @@ class DifferentialDrive : public RobotDriveBase,
                           public wpi::Sendable,
                           public wpi::SendableHelper<DifferentialDrive> {
  public:
+  /**
+   * Wheel speeds for a differential drive.
+   *
+   * Uses normalized voltage [-1.0..1.0].
+   */
   struct WheelSpeeds {
     double left = 0.0;
     double right = 0.0;
@@ -172,6 +177,7 @@ class DifferentialDrive : public RobotDriveBase,
    * @param zRotation    The rotation rate of the robot around the Z axis
    *                     [-1.0..1.0]. Clockwise is positive.
    * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   * @return Wheel speeds [-1.0..1.0].
    */
   static WheelSpeeds ArcadeDriveIK(double xSpeed, double zRotation,
                                    bool squareInputs = true);
@@ -190,6 +196,7 @@ class DifferentialDrive : public RobotDriveBase,
    * @param allowTurnInPlace If set, overrides constant-curvature turning for
    *                         turn-in-place maneuvers. zRotation will control
    *                         turning rate instead of curvature.
+   * @return Wheel speeds [-1.0..1.0].
    */
   static WheelSpeeds CurvatureDriveIK(double xSpeed, double zRotation,
                                       bool allowTurnInPlace);
@@ -202,6 +209,7 @@ class DifferentialDrive : public RobotDriveBase,
    * @param rightSpeed   The robot right side's speed along the X axis
    *                     [-1.0..1.0]. Forward is positive.
    * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   * @return Wheel speeds [-1.0..1.0].
    */
   static WheelSpeeds TankDriveIK(double leftSpeed, double rightSpeed,
                                  bool squareInputs = true);

wpilibc/src/main/native/include/frc/drive/KilloughDrive.h

@@ -62,6 +62,11 @@ class KilloughDrive : public RobotDriveBase,
   static constexpr double kDefaultRightMotorAngle = 120.0;
   static constexpr double kDefaultBackMotorAngle = 270.0;
 
+  /**
+   * Wheel speeds for a Killough drive.
+   *
+   * Uses normalized voltage [-1.0..1.0].
+   */
   struct WheelSpeeds {
     double left = 0.0;
     double right = 0.0;
@@ -154,6 +159,7 @@ class KilloughDrive : public RobotDriveBase,
    *                  Clockwise is positive.
    * @param gyroAngle The current angle reading from the gyro in degrees around
    *                  the Z axis. Use this to implement field-oriented controls.
+   * @return Wheel speeds [-1.0..1.0].
    */
   WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed, double zRotation,
                                double gyroAngle = 0.0);

wpilibc/src/main/native/include/frc/drive/MecanumDrive.h

@@ -44,25 +44,20 @@ class SpeedController;
  * </pre>
  *
  * Each Drive() function provides different inverse kinematic relations for a
- * Mecanum drive robot. Motor outputs for the right side are negated, so motor
- * direction inversion by the user is usually unnecessary.
+ * Mecanum drive robot.
  *
- * This library uses the NED axes convention (North-East-Down as external
- * reference in the world frame):
- * http://www.nuclearprojects.com/ins/images/axis_big.png.
- *
- * The positive X axis points ahead, the positive Y axis points to the right,
+ * The positive Y axis points ahead, the positive X axis points to the right,
  * and the positive Z axis points down. Rotations follow the right-hand rule, so
  * clockwise rotation around the Z axis is positive.
  *
+ * Note: the axis conventions used in this class differ from DifferentialDrive.
+ * This may change in a future year's WPILib release.
+ *
  * Inputs smaller then 0.02 will be set to 0, and larger values will be scaled
  * so that the full range is still used. This deadband value can be changed
  * with SetDeadband().
  *
  * RobotDrive porting guide:
- * <br>In MecanumDrive, the right side motor controllers are automatically
- * inverted, while in RobotDrive, no motor controllers are automatically
- * inverted.
  * <br>DriveCartesian(double, double, double, double) is equivalent to
  * RobotDrive's MecanumDrive_Cartesian(double, double, double, double)
  * if a deadband of 0 is used, and the ySpeed and gyroAngle values are inverted
@@ -76,6 +71,11 @@ class MecanumDrive : public RobotDriveBase,
                      public wpi::Sendable,
                      public wpi::SendableHelper<MecanumDrive> {
  public:
+  /**
+   * Wheel speeds for a mecanum drive.
+   *
+   * Uses normalized voltage [-1.0..1.0].
+   */
   struct WheelSpeeds {
     double frontLeft = 0.0;
     double frontRight = 0.0;
@@ -103,9 +103,9 @@ class MecanumDrive : public RobotDriveBase,
    * Angles are measured clockwise from the positive X axis. The robot's speed
    * is independent from its angle or rotation rate.
    *
-   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Right is
+   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Forward is
    *                  positive.
-   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Forward is
+   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Right is
    *                  positive.
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
    *                  Clockwise is positive.
@@ -136,14 +136,15 @@ class MecanumDrive : public RobotDriveBase,
    * Angles are measured clockwise from the positive X axis. The robot's speed
    * is independent from its angle or rotation rate.
    *
-   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Right is
+   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Forward is
    *                  positive.
-   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Forward is
+   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Right is
    *                  positive.
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
    *                  Clockwise is positive.
    * @param gyroAngle The current angle reading from the gyro in degrees around
    *                  the Z axis. Use this to implement field-oriented controls.
+   * @return Wheel speeds [-1.0..1.0].
    */
   static WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed,
                                       double zRotation, double gyroAngle = 0.0);

wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h

@@ -370,7 +370,16 @@ enum class BuiltInWidgets {
    * </td></tr>
    * </table>
    */
-  kCameraStream
+  kCameraStream,
+  /**
+   * Displays a field2d object.<br>
+   * Supported types:
+   *
+   * <ul>
+   *   <li>Field2d
+   * </ul>
+   */
+  kField,
 };
 
 }  // namespace frc

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

@@ -0,0 +1,81 @@
+// 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.
+
+#pragma once
+
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/moment_of_inertia.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated DC motor mechanism.
+ */
+class DCMotorSim : public LinearSystemSim<2, 1, 2> {
+ public:
+  /**
+   * Creates a simulated DC motor mechanism.
+   *
+   * @param plant              The linear system representing the DC motor.
+   * @param gearbox            The type of and number of motors in the DC motor
+   * gearbox.
+   * @param gearing            The gearing of the DC motor (numbers greater than
+   * 1 represent reductions).
+   * @param measurementStdDevs The standard deviation of the measurement noise.
+   */
+  DCMotorSim(const LinearSystem<2, 1, 2>& plant, const DCMotor& gearbox,
+             double gearing,
+             const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
+
+  /**
+   * Creates a simulated DC motor mechanism.
+   *
+   * @param gearbox            The type of and number of motors in the DC motor
+   * gearbox.
+   * @param gearing            The gearing of the DC motor (numbers greater than
+   * 1 represent reductions).
+   * @param moi                The moment of inertia of the DC motor.
+   * @param measurementStdDevs The standard deviation of the measurement noise.
+   */
+  DCMotorSim(const DCMotor& gearbox, double gearing,
+             units::kilogram_square_meter_t moi,
+             const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
+
+  /**
+   * Returns the DC motor position.
+   *
+   * @return The DC motor position.
+   */
+  units::radian_t GetAngularPosition() const;
+
+  /**
+   * Returns the DC motor velocity.
+   *
+   * @return The DC motor velocity.
+   */
+  units::radians_per_second_t GetAngularVelocity() const;
+
+  /**
+   * Returns the DC motor current draw.
+   *
+   * @return The DC motor current draw.
+   */
+  units::ampere_t GetCurrentDraw() const override;
+
+  /**
+   * Sets the input voltage for the DC motor.
+   *
+   * @param voltage The input voltage.
+   */
+  void SetInputVoltage(units::volt_t voltage);
+
+ private:
+  DCMotor m_gearbox;
+  double m_gearing;
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h

@@ -34,7 +34,14 @@ class MechanismLigament2d : public MechanismObject2d {
    *
    * @param color the color of the line
    */
-  void SetColor(const frc::Color8Bit& color);
+  void SetColor(const Color8Bit& color);
+
+  /**
+   * Get the ligament color.
+   *
+   * @return the color of the line
+   */
+  Color8Bit GetColor();
 
   /**
    * Set the ligament's length.
@@ -71,6 +78,13 @@ class MechanismLigament2d : public MechanismObject2d {
    */
   void SetLineWeight(double lineWidth);
 
+  /**
+   * Get the line thickness.
+   *
+   * @return the line thickness
+   */
+  double GetLineWeight();
+
  protected:
   void UpdateEntries(std::shared_ptr<nt::NetworkTable> table) override;
 

wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp

@@ -0,0 +1,90 @@
+// 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/Encoder.h"
+#include "frc/RobotController.h"
+#include "frc/controller/PIDController.h"
+#include "frc/motorcontrol/PWMVictorSPX.h"
+#include "frc/simulation/BatterySim.h"
+#include "frc/simulation/DCMotorSim.h"
+#include "frc/simulation/EncoderSim.h"
+#include "frc/simulation/RoboRioSim.h"
+#include "gtest/gtest.h"
+
+TEST(DCMotorSimTest, VoltageSteadyState) {
+  frc::DCMotor gearbox = frc::DCMotor::NEO(1);
+  frc::sim::DCMotorSim sim{gearbox, 1.0,
+                           units::kilogram_square_meter_t{0.0005}};
+
+  frc::Encoder encoder{0, 1};
+  frc::sim::EncoderSim encoderSim{encoder};
+  frc::PWMVictorSPX motor{0};
+
+  frc::sim::RoboRioSim::ResetData();
+  encoderSim.ResetData();
+
+  // Spin-up
+  for (int i = 0; i < 100; i++) {
+    // RobotPeriodic runs first
+    motor.SetVoltage(12_V);
+
+    // Then, SimulationPeriodic runs
+    frc::sim::RoboRioSim::SetVInVoltage(
+        frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
+    sim.SetInputVoltage(motor.Get() *
+                        frc::RobotController::GetBatteryVoltage());
+    sim.Update(20_ms);
+    encoderSim.SetRate(sim.GetAngularVelocity().value());
+  }
+
+  EXPECT_NEAR((gearbox.Kv * 12_V).value(), encoder.GetRate(), 0.1);
+
+  // Decay
+  for (int i = 0; i < 100; i++) {
+    // RobotPeriodic runs first
+    motor.SetVoltage(0_V);
+
+    // Then, SimulationPeriodic runs
+    frc::sim::RoboRioSim::SetVInVoltage(
+        frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
+    sim.SetInputVoltage(motor.Get() *
+                        frc::RobotController::GetBatteryVoltage());
+    sim.Update(20_ms);
+    encoderSim.SetRate(sim.GetAngularVelocity().value());
+  }
+
+  EXPECT_NEAR(0, encoder.GetRate(), 0.1);
+}
+
+TEST(DCMotorSimTest, PositionFeedbackControl) {
+  frc::DCMotor gearbox = frc::DCMotor::NEO(1);
+  frc::sim::DCMotorSim sim{gearbox, 1.0,
+                           units::kilogram_square_meter_t{0.0005}};
+
+  frc2::PIDController controller{0.04, 0.0, 0.001};
+
+  frc::Encoder encoder{0, 1};
+  frc::sim::EncoderSim encoderSim{encoder};
+  frc::PWMVictorSPX motor{0};
+
+  frc::sim::RoboRioSim::ResetData();
+  encoderSim.ResetData();
+
+  for (int i = 0; i < 140; i++) {
+    // RobotPeriodic runs first
+    motor.Set(controller.Calculate(encoder.GetDistance(), 750));
+
+    // Then, SimulationPeriodic runs
+    frc::sim::RoboRioSim::SetVInVoltage(
+        frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
+    sim.SetInputVoltage(motor.Get() *
+                        frc::RobotController::GetBatteryVoltage());
+    sim.Update(20_ms);
+    encoderSim.SetDistance(sim.GetAngularPosition().value());
+    encoderSim.SetRate(sim.GetAngularVelocity().value());
+  }
+
+  EXPECT_NEAR(encoder.GetDistance(), 750, 1.0);
+  EXPECT_NEAR(encoder.GetRate(), 0, 0.1);
+}

wpilibcExamples/build.gradle

@@ -39,6 +39,12 @@ nativeUtils.platformConfigs.named(nativeUtils.wpi.platforms.roborio).configure {
     cppCompiler.args.add('-Werror=deprecated-declarations')
 }
 
+nativeUtils.platformConfigs.named(nativeUtils.wpi.platforms.windowsx64).configure {
+    linker.args.remove('/DEBUG:FULL')
+    cppCompiler.debugArgs.remove('/Zi')
+    cCompiler.debugArgs.remove('/Zi')
+}
+
 ext {
     sharedCvConfigs = examplesMap + templatesMap + [commands: []]
     staticCvConfigs = [:]

wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp

@@ -15,7 +15,7 @@ ArmSubsystem::ArmSubsystem()
               kP, 0, 0, {kMaxVelocity, kMaxAcceleration})),
       m_motor(kMotorPort),
       m_encoder(kEncoderPorts[0], kEncoderPorts[1]),
-      m_feedforward(kS, kCos, kV, kA) {
+      m_feedforward(kS, kG, kV, kA) {
   m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
   // Start arm in neutral position
   SetGoal(State{kArmOffset, 0_rad_per_s});

wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h

@@ -46,7 +46,7 @@ constexpr double kP = 1;
 // These are fake gains; in actuality these must be determined individually for
 // each robot
 constexpr auto kS = 1_V;
-constexpr auto kCos = 1_V;
+constexpr auto kG = 1_V;
 constexpr auto kV = 0.5_V * 1_s / 1_rad;
 constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
 

wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp

@@ -13,7 +13,7 @@ ArmSubsystem::ArmSubsystem()
     : frc2::TrapezoidProfileSubsystem<units::radians>(
           {kMaxVelocity, kMaxAcceleration}, kArmOffset),
       m_motor(kMotorPort),
-      m_feedforward(kS, kCos, kV, kA) {
+      m_feedforward(kS, kG, kV, kA) {
   m_motor.SetPID(kP, 0, 0);
 }
 

wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h

@@ -46,7 +46,7 @@ constexpr double kP = 1;
 // These are fake gains; in actuality these must be determined individually for
 // each robot
 constexpr auto kS = 1_V;
-constexpr auto kCos = 1_V;
+constexpr auto kG = 1_V;
 constexpr auto kV = 0.5_V * 1_s / 1_rad;
 constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
 

wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp

@@ -4,6 +4,7 @@
 
 #include <frc/Encoder.h>
 #include <frc/Joystick.h>
+#include <frc/Preferences.h>
 #include <frc/RobotController.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/PIDController.h>
@@ -33,8 +34,13 @@ class Robot : public frc::TimedRobot {
   static constexpr int kEncoderBChannel = 1;
   static constexpr int kJoystickPort = 0;
 
+  static constexpr std::string_view kArmPositionKey = "ArmPosition";
+  static constexpr std::string_view kArmPKey = "ArmP";
+
   // The P gain for the PID controller that drives this arm.
-  static constexpr double kArmKp = 50.0;
+  double kArmKp = 50.0;
+
+  units::degree_t armPosition = 75.0_deg;
 
   // distance per pulse = (angle per revolution) / (pulses per revolution)
   //  = (2 * PI rads) / (4096 pulses)
@@ -81,6 +87,15 @@ class Robot : public frc::TimedRobot {
 
     // Put Mechanism 2d to SmartDashboard
     frc::SmartDashboard::PutData("Arm Sim", &m_mech2d);
+
+    // Set the Arm position setpoint and P constant to Preferences if the keys
+    // don't already exist
+    if (!frc::Preferences::ContainsKey(kArmPositionKey)) {
+      frc::Preferences::SetDouble(kArmPositionKey, armPosition.value());
+    }
+    if (!frc::Preferences::ContainsKey(kArmPKey)) {
+      frc::Preferences::SetDouble(kArmPKey, kArmKp);
+    }
   }
 
   void SimulationPeriodic() override {
@@ -103,12 +118,22 @@ class Robot : public frc::TimedRobot {
     m_arm->SetAngle(m_armSim.GetAngle());
   }
 
+  void TeleopInit() override {
+    // Read Preferences for Arm setpoint and kP on entering Teleop
+    armPosition = units::degree_t(
+        frc::Preferences::GetDouble(kArmPositionKey, armPosition.value()));
+    if (kArmKp != frc::Preferences::GetDouble(kArmPKey, kArmKp)) {
+      kArmKp = frc::Preferences::GetDouble(kArmPKey, kArmKp);
+      m_controller.SetP(kArmKp);
+    }
+  }
+
   void TeleopPeriodic() override {
     if (m_joystick.GetTrigger()) {
-      // Here, we run PID control like normal, with a constant setpoint of 75
-      // degrees.
+      // Here, we run PID control like normal, with a setpoint read from
+      // preferences in degrees.
       double pidOutput = m_controller.Calculate(
-          m_encoder.GetDistance(), (units::radian_t(75_deg)).value());
+          m_encoder.GetDistance(), (units::radian_t(armPosition).value()));
       m_motor.SetVoltage(units::volt_t(pidOutput));
     } else {
       // Otherwise, we disable the motor.

wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp

@@ -10,8 +10,8 @@
 
 /**
  * This is a sample program that uses mecanum drive with a gyro sensor to
- * maintain rotation vectorsin relation to the starting orientation of the robot
- * (field-oriented controls).
+ * maintain rotation vectors in relation to the starting orientation of the
+ * robot (field-oriented controls).
  */
 class Robot : public frc::TimedRobot {
  public:
@@ -28,7 +28,7 @@ class Robot : public frc::TimedRobot {
    * Mecanum drive is used with the gyro angle as an input.
    */
   void TeleopPeriodic() override {
-    m_robotDrive.DriveCartesian(m_joystick.GetX(), m_joystick.GetY(),
+    m_robotDrive.DriveCartesian(-m_joystick.GetY(), m_joystick.GetX(),
                                 m_joystick.GetZ(), m_gyro.GetAngle());
   }
 

wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp

@@ -24,7 +24,8 @@ class Robot : public frc::TimedRobot {
     /* Use the joystick X axis for lateral movement, Y axis for forward
      * movement, and Z axis for rotation.
      */
-    m_robotDrive.DriveCartesian(m_stick.GetX(), m_stick.GetY(), m_stick.GetZ());
+    m_robotDrive.DriveCartesian(-m_stick.GetY(), m_stick.GetX(),
+                                m_stick.GetZ());
   }
 
  private:

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -713,7 +713,8 @@
       "Sensors",
       "Simulation",
       "Physics",
-      "Mechanism2d"
+      "Mechanism2d",
+      "Preferences"
     ],
     "foldername": "ArmSimulation",
     "gradlebase": "cpp",

wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp

@@ -65,6 +65,16 @@ void Robot::TeleopPeriodic() {}
  */
 void Robot::TestPeriodic() {}
 
+/**
+ * This function is called once when the robot is first started up.
+ */
+void Robot::SimulationInit() {}
+
+/**
+ * This function is called periodically whilst in simulation.
+ */
+void Robot::SimulationPeriodic() {}
+
 #ifndef RUNNING_FRC_TESTS
 int main() {
   return frc::StartRobot<Robot>();

wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h

@@ -20,6 +20,8 @@ class Robot : public frc::TimedRobot {
   void TeleopInit() override;
   void TeleopPeriodic() override;
   void TestPeriodic() override;
+  void SimulationInit() override;
+  void SimulationPeriodic() override;
 
  private:
   // Have it null by default so that if testing teleop it

wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp

@@ -68,6 +68,10 @@ void Robot::TestInit() {}
 
 void Robot::TestPeriodic() {}
 
+void Robot::SimulationInit() {}
+
+void Robot::SimulationPeriodic() {}
+
 #ifndef RUNNING_FRC_TESTS
 int main() {
   return frc::StartRobot<Robot>();

wpilibcExamples/src/main/cpp/templates/timed/include/Robot.h

@@ -21,6 +21,8 @@ class Robot : public frc::TimedRobot {
   void DisabledPeriodic() override;
   void TestInit() override;
   void TestPeriodic() override;
+  void SimulationInit() override;
+  void SimulationPeriodic() override;
 
  private:
   frc::SendableChooser<std::string> m_chooser;

wpilibcExamples/src/main/cpp/templates/timedskeleton/cpp/Robot.cpp

@@ -19,6 +19,9 @@ void Robot::DisabledPeriodic() {}
 void Robot::TestInit() {}
 void Robot::TestPeriodic() {}
 
+void Robot::SimulationInit() {}
+void Robot::SimulationPeriodic() {}
+
 #ifndef RUNNING_FRC_TESTS
 int main() {
   return frc::StartRobot<Robot>();

wpilibcExamples/src/main/cpp/templates/timedskeleton/include/Robot.h

@@ -22,4 +22,7 @@ class Robot : public frc::TimedRobot {
 
   void TestInit() override;
   void TestPeriodic() override;
+
+  void SimulationInit() override;
+  void SimulationPeriodic() override;
 };

wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java

@@ -961,11 +961,11 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable {
   public synchronized double getRate() {
     switch (m_yaw_axis) {
       case kX:
-        return getGyroAngleX();
+        return getGyroRateX();
       case kY:
-        return getGyroAngleY();
+        return getGyroRateY();
       case kZ:
-        return getGyroAngleZ();
+        return getGyroRateZ();
       default:
         return 0.0;
     }

wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java

@@ -9,6 +9,11 @@ import edu.wpi.first.hal.REVPHFaults;
 import edu.wpi.first.hal.REVPHJNI;
 import edu.wpi.first.hal.REVPHStickyFaults;
 import edu.wpi.first.hal.REVPHVersion;
+import java.io.File;
+import java.io.IOException;
+import java.io.OutputStream;
+import java.nio.charset.StandardCharsets;
+import java.nio.file.Files;
 import java.util.HashMap;
 import java.util.Map;
 
@@ -29,9 +34,38 @@ public class PneumaticHub implements PneumaticsBase {
       m_handleMap.put(module, this);
 
       final REVPHVersion version = REVPHJNI.getVersion(m_handle);
+      final String fwVersion =
+          version.firmwareMajor + "." + version.firmwareMinor + "." + version.firmwareFix;
+
+      if (version.firmwareMajor > 0 && RobotBase.isReal()) {
+        // Write PH firmware version to roboRIO
+        final String fileName = "REV_PH_" + String.format("%02d", module) + "_WPILib_Version.ini";
+        final File file = new File("/tmp/frc_versions/" + fileName);
+        try {
+          if (file.exists() && !file.delete()) {
+            throw new IOException("Failed to delete " + fileName);
+          }
+
+          if (!file.createNewFile()) {
+            throw new IOException("Failed to create new " + fileName);
+          }
+
+          try (OutputStream output = Files.newOutputStream(file.toPath())) {
+            output.write("[Version]\n".getBytes(StandardCharsets.UTF_8));
+            output.write("model=REV PH\n".getBytes(StandardCharsets.UTF_8));
+            output.write(
+                ("deviceID=" + Integer.toHexString(0x9052600 | module) + "\n")
+                    .getBytes(StandardCharsets.UTF_8));
+            output.write(("currentVersion=" + fwVersion).getBytes(StandardCharsets.UTF_8));
+          }
+        } catch (IOException ex) {
+          DriverStation.reportError(
+              "Could not write " + fileName + ": " + ex.toString(), ex.getStackTrace());
+        }
+      }
+
+      // Check PH firmware version
       if (version.firmwareMajor > 0 && version.firmwareMajor < 22) {
-        final String fwVersion =
-            version.firmwareMajor + "." + version.firmwareMinor + "." + version.firmwareFix;
         throw new IllegalStateException(
             "The Pneumatic Hub has firmware version "
                 + fwVersion
@@ -280,35 +314,35 @@ public class PneumaticHub implements PneumaticsBase {
     return voltsToPsi(sensorVoltage, supplyVoltage);
   }
 
-  void clearStickyFaults() {
+  public void clearStickyFaults() {
     REVPHJNI.clearStickyFaults(m_handle);
   }
 
-  REVPHVersion getVersion() {
+  public REVPHVersion getVersion() {
     return REVPHJNI.getVersion(m_handle);
   }
 
-  REVPHFaults getFaults() {
+  public REVPHFaults getFaults() {
     return REVPHJNI.getFaults(m_handle);
   }
 
-  REVPHStickyFaults getStickyFaults() {
+  public REVPHStickyFaults getStickyFaults() {
     return REVPHJNI.getStickyFaults(m_handle);
   }
 
-  double getInputVoltage() {
+  public double getInputVoltage() {
     return REVPHJNI.getInputVoltage(m_handle);
   }
 
-  double get5VRegulatedVoltage() {
+  public double get5VRegulatedVoltage() {
     return REVPHJNI.get5VVoltage(m_handle);
   }
 
-  double getSolenoidsTotalCurrent() {
+  public double getSolenoidsTotalCurrent() {
     return REVPHJNI.getSolenoidCurrent(m_handle);
   }
 
-  double getSolenoidsVoltage() {
+  public double getSolenoidsVoltage() {
     return REVPHJNI.getSolenoidVoltage(m_handle);
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java

@@ -15,8 +15,8 @@ import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
- * Class for getting voltage, current, temperature, power and energy from the Power Distribution
- * Panel over CAN.
+ * Class for getting voltage, current, temperature, power and energy from the CTRE Power
+ * Distribution Panel (PDP) or REV Power Distribution Hub (PDH) over CAN.
  */
 public class PowerDistribution implements Sendable, AutoCloseable {
   private final int m_handle;
@@ -36,9 +36,9 @@ public class PowerDistribution implements Sendable, AutoCloseable {
   }
 
   /**
-   * Constructs a PowerDistribution.
+   * Constructs a PowerDistribution object.
    *
-   * @param module The CAN ID of the PDP.
+   * @param module The CAN ID of the PDP/PDH.
    * @param moduleType Module type (CTRE or REV).
    */
   public PowerDistribution(int module, ModuleType moduleType) {
@@ -50,9 +50,9 @@ public class PowerDistribution implements Sendable, AutoCloseable {
   }
 
   /**
-   * Constructs a PowerDistribution.
+   * Constructs a PowerDistribution object.
    *
-   * <p>Uses the default CAN ID (0 for CTRE and 1 for REV).
+   * <p>Detects the connected PDP/PDH using the default CAN ID (0 for CTRE and 1 for REV).
    */
   public PowerDistribution() {
     m_handle = PowerDistributionJNI.initialize(kDefaultModule, PowerDistributionJNI.AUTOMATIC_TYPE);
@@ -68,37 +68,37 @@ public class PowerDistribution implements Sendable, AutoCloseable {
   }
 
   /**
-   * Gets the number of channel for this power distribution.
+   * Gets the number of channels for this power distribution object.
    *
-   * @return Number of output channels.
+   * @return Number of output channels (16 for PDP, 24 for PDH).
    */
   public int getNumChannels() {
     return PowerDistributionJNI.getNumChannels(m_handle);
   }
 
   /**
-   * Query the input voltage of the PDP.
+   * Query the input voltage of the PDP/PDH.
    *
-   * @return The voltage of the PDP in volts
+   * @return The voltage in volts
    */
   public double getVoltage() {
     return PowerDistributionJNI.getVoltage(m_handle);
   }
 
   /**
-   * Query the temperature of the PDP.
+   * Query the temperature of the PDP/PDH.
    *
-   * @return The temperature of the PDP in degrees Celsius
+   * @return The temperature in degrees Celsius
    */
   public double getTemperature() {
     return PowerDistributionJNI.getTemperature(m_handle);
   }
 
   /**
-   * Query the current of a single channel of the PDP.
+   * Query the current of a single channel of the PDP/PDH.
    *
-   * @param channel The PDP channel to query.
-   * @return The current of one of the PDP channels (channels 0-15) in Amperes
+   * @param channel The channel (0-15 for PDP, 0-23 for PDH) to query
+   * @return The current of the channel in Amperes
    */
   public double getCurrent(int channel) {
     double current = PowerDistributionJNI.getChannelCurrent(m_handle, channel);
@@ -107,7 +107,7 @@ public class PowerDistribution implements Sendable, AutoCloseable {
   }
 
   /**
-   * Query the current of all monitored PDP channels (0-15).
+   * Query the current of all monitored channels.
    *
    * @return The current of all the channels in Amperes
    */
@@ -116,7 +116,7 @@ public class PowerDistribution implements Sendable, AutoCloseable {
   }
 
   /**
-   * Query the total power drawn from the monitored PDP channels.
+   * Query the total power drawn from the monitored channels.
    *
    * @return the total power in Watts
    */
@@ -125,7 +125,7 @@ public class PowerDistribution implements Sendable, AutoCloseable {
   }
 
   /**
-   * Query the total energy drawn from the monitored PDP channels.
+   * Query the total energy drawn from the monitored channels.
    *
    * @return the total energy in Joules
    */
@@ -138,7 +138,7 @@ public class PowerDistribution implements Sendable, AutoCloseable {
     PowerDistributionJNI.resetTotalEnergy(m_handle);
   }
 
-  /** Clear all PDP sticky faults. */
+  /** Clear all PDP/PDH sticky faults. */
   public void clearStickyFaults() {
     PowerDistributionJNI.clearStickyFaults(m_handle);
   }
@@ -152,23 +152,47 @@ public class PowerDistribution implements Sendable, AutoCloseable {
     return m_module;
   }
 
+  /**
+   * Gets the module type for this power distribution object.
+   *
+   * @return The module type
+   */
+  public ModuleType getType() {
+    int type = PowerDistributionJNI.getType(m_handle);
+    if (type == PowerDistributionJNI.REV_TYPE) {
+      return ModuleType.kRev;
+    } else {
+      return ModuleType.kCTRE;
+    }
+  }
+
+  /**
+   * Gets whether the PDH switchable channel is turned on or off. Returns false with the CTRE PDP.
+   *
+   * @return The output state of the PDH switchable channel
+   */
   public boolean getSwitchableChannel() {
     return PowerDistributionJNI.getSwitchableChannel(m_handle);
   }
 
+  /**
+   * Sets the PDH switchable channel on or off. Does nothing with the CTRE PDP.
+   *
+   * @param enabled Whether to turn the PDH switchable channel on or off
+   */
   public void setSwitchableChannel(boolean enabled) {
     PowerDistributionJNI.setSwitchableChannel(m_handle, enabled);
   }
 
-  PowerDistributionVersion getVersion() {
+  public PowerDistributionVersion getVersion() {
     return PowerDistributionJNI.getVersion(m_handle);
   }
 
-  PowerDistributionFaults getFaults() {
+  public PowerDistributionFaults getFaults() {
     return PowerDistributionJNI.getFaults(m_handle);
   }
 
-  PowerDistributionStickyFaults getStickyFaults() {
+  public PowerDistributionStickyFaults getStickyFaults() {
     return PowerDistributionJNI.getStickyFaults(m_handle);
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java

@@ -94,6 +94,11 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
 
   private boolean m_reported;
 
+  /**
+   * Wheel speeds for a differential drive.
+   *
+   * <p>Uses normalized voltage [-1.0..1.0].
+   */
   @SuppressWarnings("MemberName")
   public static class WheelSpeeds {
     public double left;
@@ -105,8 +110,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
     /**
      * Constructs a WheelSpeeds.
      *
-     * @param left The left speed.
-     * @param right The right speed.
+     * @param left The left speed [-1.0..1.0].
+     * @param right The right speed [-1.0..1.0].
      */
     public WheelSpeeds(double left, double right) {
       this.left = left;
@@ -256,7 +261,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
    *     positive.
    * @param squareInputs If set, decreases the input sensitivity at low speeds.
-   * @return Wheel speeds.
+   * @return Wheel speeds [-1.0..1.0].
    */
   @SuppressWarnings("ParameterName")
   public static WheelSpeeds arcadeDriveIK(double xSpeed, double zRotation, boolean squareInputs) {
@@ -315,7 +320,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
    * @param zRotation The normalized curvature [-1.0..1.0]. Clockwise is positive.
    * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
    *     maneuvers. zRotation will control rotation rate around the Z axis instead of curvature.
-   * @return Wheel speeds.
+   * @return Wheel speeds [-1.0..1.0].
    */
   @SuppressWarnings("ParameterName")
   public static WheelSpeeds curvatureDriveIK(
@@ -351,7 +356,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
    * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
    *     positive.
    * @param squareInputs If set, decreases the input sensitivity at low speeds.
-   * @return Wheel speeds.
+   * @return Wheel speeds [-1.0..1.0].
    */
   public static WheelSpeeds tankDriveIK(double leftSpeed, double rightSpeed, boolean squareInputs) {
     leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0);

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java

@@ -58,6 +58,11 @@ public class KilloughDrive extends RobotDriveBase implements Sendable, AutoClose
 
   private boolean m_reported;
 
+  /**
+   * Wheel speeds for a Killough drive.
+   *
+   * <p>Uses normalized voltage [-1.0..1.0].
+   */
   @SuppressWarnings("MemberName")
   public static class WheelSpeeds {
     public double left;
@@ -70,9 +75,9 @@ public class KilloughDrive extends RobotDriveBase implements Sendable, AutoClose
     /**
      * Constructs a WheelSpeeds.
      *
-     * @param left The left speed.
-     * @param right The right speed.
-     * @param back The back speed.
+     * @param left The left speed [-1.0..1.0].
+     * @param right The right speed [-1.0..1.0].
+     * @param back The back speed [-1.0..1.0].
      */
     public WheelSpeeds(double left, double right, double back) {
       this.left = left;
@@ -240,7 +245,7 @@ public class KilloughDrive extends RobotDriveBase implements Sendable, AutoClose
    *     positive.
    * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
    *     to implement field-oriented controls.
-   * @return Wheel speeds.
+   * @return Wheel speeds [-1.0..1.0].
    */
   @SuppressWarnings("ParameterName")
   public WheelSpeeds driveCartesianIK(

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java

@@ -34,23 +34,20 @@ import edu.wpi.first.wpilibj.SpeedController;
  * </pre>
  *
  * <p>Each drive() function provides different inverse kinematic relations for a Mecanum drive
- * robot. Motor outputs for the right side are negated, so motor direction inversion by the user is
- * usually unnecessary.
+ * robot.
  *
- * <p>This library uses the NED axes convention (North-East-Down as external reference in the world
- * frame): http://www.nuclearprojects.com/ins/images/axis_big.png.
- *
- * <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
+ * <p>The positive Y axis points ahead, the positive X axis points right, and the positive Z axis
  * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
  * positive.
  *
+ * <p>Note: the axis conventions used in this class differ from DifferentialDrive. This may change
+ * in a future year's WPILib release.
+ *
  * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
  * be set to 0, and larger values will be scaled so that the full range is still used. This deadband
  * value can be changed with {@link #setDeadband}.
  *
  * <p>RobotDrive porting guide: <br>
- * In MecanumDrive, the right side motor controllers are automatically inverted, while in
- * RobotDrive, no motor controllers are automatically inverted. <br>
  * {@link #driveCartesian(double, double, double, double)} is equivalent to RobotDrive's
  * mecanumDrive_Cartesian(double, double, double, double) if a deadband of 0 is used, and the ySpeed
  * and gyroAngle values are inverted compared to RobotDrive (eg driveCartesian(xSpeed, -ySpeed,
@@ -69,6 +66,11 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
 
   private boolean m_reported;
 
+  /**
+   * Wheel speeds for a mecanum drive.
+   *
+   * <p>Uses normalized voltage [-1.0..1.0].
+   */
   @SuppressWarnings("MemberName")
   public static class WheelSpeeds {
     public double frontLeft;
@@ -82,10 +84,10 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
     /**
      * Constructs a WheelSpeeds.
      *
-     * @param frontLeft The front left speed.
-     * @param frontRight The front right speed.
-     * @param rearLeft The rear left speed.
-     * @param rearRight The rear right speed.
+     * @param frontLeft The front left speed [-1.0..1.0].
+     * @param frontRight The front right speed [-1.0..1.0].
+     * @param rearLeft The rear left speed [-1.0..1.0].
+     * @param rearRight The rear right speed [-1.0..1.0].
      */
     public WheelSpeeds(double frontLeft, double frontRight, double rearLeft, double rearRight) {
       this.frontLeft = frontLeft;
@@ -138,8 +140,8 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
    * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
    * from its angle or rotation rate.
    *
-   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
-   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Forward is positive.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is positive.
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
    *     positive.
    */
@@ -154,8 +156,8 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
    * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
    * from its angle or rotation rate.
    *
-   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
-   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Forward is positive.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is positive.
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
    *     positive.
    * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
@@ -213,13 +215,13 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
    * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
    * from its angle or rotation rate.
    *
-   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
-   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Forward is positive.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is positive.
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
    *     positive.
    * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
    *     to implement field-oriented controls.
-   * @return Wheel speeds.
+   * @return Wheel speeds [-1.0..1.0].
    */
   @SuppressWarnings("ParameterName")
   public static WheelSpeeds driveCartesianIK(

wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java

@@ -473,6 +473,15 @@ public enum BuiltInWidgets implements WidgetType {
    * </table>
    */
   kCameraStream("Camera Stream"),
+  /**
+   * Displays a field2d object.<br>
+   * Supported types:
+   *
+   * <ul>
+   *   <li>{@link edu.wpi.first.wpilibj.smartdashboard.Field2d}
+   * </ul>
+   */
+  kField("Field"),
   ;
 
   private final String m_widgetName;

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java

@@ -0,0 +1,145 @@
+// 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.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
+
+/** Represents a simulated DC motor mechanism. */
+public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
+  // Gearbox for the DC motor.
+  private final DCMotor m_gearbox;
+
+  // The gearing from the motors to the output.
+  private final double m_gearing;
+
+  /**
+   * Creates a simulated DC motor mechanism.
+   *
+   * @param plant The linear system that represents the DC motor.
+   * @param gearbox The type of and number of motors in the DC motor gearbox.
+   * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
+   */
+  public DCMotorSim(LinearSystem<N2, N1, N2> plant, DCMotor gearbox, double gearing) {
+    super(plant);
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+  }
+
+  /**
+   * Creates a simulated DC motor mechanism.
+   *
+   * @param plant The linear system that represents the DC motor.
+   * @param gearbox The type of and number of motors in the DC motor gearbox.
+   * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
+   * @param measurementStdDevs The standard deviations of the measurements.
+   */
+  public DCMotorSim(
+      LinearSystem<N2, N1, N2> plant,
+      DCMotor gearbox,
+      double gearing,
+      Matrix<N2, N1> measurementStdDevs) {
+    super(plant, measurementStdDevs);
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+  }
+
+  /**
+   * Creates a simulated DC motor mechanism.
+   *
+   * @param gearbox The type of and number of motors in the DC motor gearbox.
+   * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
+   * @param jKgMetersSquared The moment of inertia of the DC motor. If this is unknown, use the
+   *     {@link #DCMotorSim(LinearSystem, DCMotor, double, Matrix)} constructor.
+   */
+  @SuppressWarnings("ParameterName")
+  public DCMotorSim(DCMotor gearbox, double gearing, double jKgMetersSquared) {
+    super(LinearSystemId.createDCMotorSystem(gearbox, jKgMetersSquared, gearing));
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+  }
+
+  /**
+   * Creates a simulated DC motor mechanism.
+   *
+   * @param gearbox The type of and number of motors in the DC motor gearbox.
+   * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
+   * @param jKgMetersSquared The moment of inertia of the DC motor. If this is unknown, use the
+   *     {@link #DCMotorSim(LinearSystem, DCMotor, double, Matrix)} constructor.
+   * @param measurementStdDevs The standard deviations of the measurements.
+   */
+  @SuppressWarnings("ParameterName")
+  public DCMotorSim(
+      DCMotor gearbox, double gearing, double jKgMetersSquared, Matrix<N2, N1> measurementStdDevs) {
+    super(
+        LinearSystemId.createDCMotorSystem(gearbox, jKgMetersSquared, gearing), measurementStdDevs);
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+  }
+
+  /**
+   * Returns the DC motor position.
+   *
+   * @return The DC motor position.
+   */
+  public double getAngularPositionRad() {
+    return getOutput(0);
+  }
+
+  /**
+   * Returns the DC motor position in rotations.
+   *
+   * @return The DC motor position in rotations.
+   */
+  public double getAngularPositionRotations() {
+    return Units.radiansToRotations(getAngularPositionRad());
+  }
+
+  /**
+   * Returns the DC motor velocity.
+   *
+   * @return The DC motor velocity.
+   */
+  public double getAngularVelocityRadPerSec() {
+    return getOutput(1);
+  }
+
+  /**
+   * Returns the DC motor velocity in RPM.
+   *
+   * @return The DC motor velocity in RPM.
+   */
+  public double getAngularVelocityRPM() {
+    return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec());
+  }
+
+  /**
+   * Returns the DC motor current draw.
+   *
+   * @return The DC motor current draw.
+   */
+  @Override
+  public double getCurrentDrawAmps() {
+    // I = V / R - omega / (Kv * R)
+    // Reductions are output over input, so a reduction of 2:1 means the motor is spinning
+    // 2x faster than the output
+    return m_gearbox.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0))
+        * Math.signum(m_u.get(0, 0));
+  }
+
+  /**
+   * Sets the input voltage for the DC motor.
+   *
+   * @param volts The input voltage.
+   */
+  public void setInputVoltage(double volts) {
+    setInput(volts);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java

@@ -117,6 +117,32 @@ public class MechanismLigament2d extends MechanismObject2d {
     flush();
   }
 
+  /**
+   * Get the ligament color.
+   *
+   * @return the color of the line
+   */
+  public synchronized Color8Bit getColor() {
+    if (m_colorEntry != null) {
+      m_color = m_colorEntry.getString("");
+    }
+    int r = 0;
+    int g = 0;
+    int b = 0;
+    if (m_color.length() == 7 && m_color.charAt(0) == '#') {
+      try {
+        r = Integer.parseInt(m_color.substring(1, 3), 16);
+        g = Integer.parseInt(m_color.substring(3, 5), 16);
+        b = Integer.parseInt(m_color.substring(5, 7), 16);
+      } catch (NumberFormatException e) {
+        r = 0;
+        g = 0;
+        b = 0;
+      }
+    }
+    return new Color8Bit(r, g, b);
+  }
+
   /**
    * Set the line thickness.
    *
@@ -127,6 +153,18 @@ public class MechanismLigament2d extends MechanismObject2d {
     flush();
   }
 
+  /**
+   * Get the line thickness.
+   *
+   * @return the line thickness
+   */
+  public synchronized double getLineWeight() {
+    if (m_weightEntry != null) {
+      m_weight = m_weightEntry.getDouble(0.0);
+    }
+    return m_weight;
+  }
+
   @Override
   protected void updateEntries(NetworkTable table) {
     table.getEntry(".type").setString("line");

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java

@@ -0,0 +1,86 @@
+// 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.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
+import org.junit.jupiter.api.Test;
+
+class DCMotorSimTest {
+  @Test
+  void testVoltageSteadyState() {
+    RoboRioSim.resetData();
+
+    DCMotor gearbox = DCMotor.getNEO(1);
+    DCMotorSim sim = new DCMotorSim(gearbox, 1.0, 0.0005);
+
+    try (var motor = new PWMVictorSPX(0);
+        var encoder = new Encoder(0, 1)) {
+      var encoderSim = new EncoderSim(encoder);
+      encoderSim.resetData();
+
+      for (int i = 0; i < 100; i++) {
+        motor.setVoltage(12);
+
+        // ------ SimulationPeriodic() happens after user code -------
+        RoboRioSim.setVInVoltage(
+            BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps()));
+        sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage());
+        sim.update(0.020);
+        encoderSim.setRate(sim.getAngularVelocityRadPerSec());
+      }
+
+      assertEquals(gearbox.KvRadPerSecPerVolt * 12, encoder.getRate(), 0.1);
+
+      for (int i = 0; i < 100; i++) {
+        motor.setVoltage(0);
+
+        // ------ SimulationPeriodic() happens after user code -------
+        RoboRioSim.setVInVoltage(
+            BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps()));
+        sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage());
+        sim.update(0.020);
+        encoderSim.setRate(sim.getAngularVelocityRadPerSec());
+      }
+
+      assertEquals(0, encoder.getRate(), 0.1);
+    }
+  }
+
+  @Test
+  void testPositionFeedbackControl() {
+    RoboRioSim.resetData();
+
+    DCMotor gearbox = DCMotor.getNEO(1);
+    DCMotorSim sim = new DCMotorSim(gearbox, 1.0, 0.0005);
+
+    try (var motor = new PWMVictorSPX(0);
+        var encoder = new Encoder(0, 1);
+        var controller = new PIDController(0.04, 0.0, 0.001); ) {
+      var encoderSim = new EncoderSim(encoder);
+      encoderSim.resetData();
+
+      for (int i = 0; i < 140; i++) {
+        motor.set(controller.calculate(encoder.getDistance(), 750));
+
+        // ------ SimulationPeriodic() happens after user code -------
+        RoboRioSim.setVInVoltage(
+            BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps()));
+        sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage());
+        sim.update(0.020);
+        encoderSim.setDistance(sim.getAngularPositionRad());
+        encoderSim.setRate(sim.getAngularVelocityRadPerSec());
+      }
+
+      assertEquals(750, encoder.getDistance(), 1.0);
+      assertEquals(0, encoder.getRate(), 0.1);
+    }
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java

@@ -38,7 +38,7 @@ public final class Constants {
 
     // These are fake gains; in actuality these must be determined individually for each robot
     public static final double kSVolts = 1;
-    public static final double kCosVolts = 1;
+    public static final double kGVolts = 1;
     public static final double kVVoltSecondPerRad = 0.5;
     public static final double kAVoltSecondSquaredPerRad = 0.1;
 

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java

@@ -19,7 +19,7 @@ public class ArmSubsystem extends ProfiledPIDSubsystem {
       new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]);
   private final ArmFeedforward m_feedforward =
       new ArmFeedforward(
-          ArmConstants.kSVolts, ArmConstants.kCosVolts,
+          ArmConstants.kSVolts, ArmConstants.kGVolts,
           ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
 
   /** Create a new ArmSubsystem. */

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java

@@ -38,7 +38,7 @@ public final class Constants {
 
     // These are fake gains; in actuality these must be determined individually for each robot
     public static final double kSVolts = 1;
-    public static final double kCosVolts = 1;
+    public static final double kGVolts = 1;
     public static final double kVVoltSecondPerRad = 0.5;
     public static final double kAVoltSecondSquaredPerRad = 0.1;
 

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java

@@ -16,7 +16,7 @@ public class ArmSubsystem extends TrapezoidProfileSubsystem {
       new ExampleSmartMotorController(ArmConstants.kMotorPort);
   private final ArmFeedforward m_feedforward =
       new ArmFeedforward(
-          ArmConstants.kSVolts, ArmConstants.kCosVolts,
+          ArmConstants.kSVolts, ArmConstants.kGVolts,
           ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
 
   /** Create a new ArmSubsystem. */

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java

@@ -10,6 +10,7 @@ import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
@@ -31,8 +32,13 @@ public class Robot extends TimedRobot {
   private static final int kEncoderBChannel = 1;
   private static final int kJoystickPort = 0;
 
+  public static final String kArmPositionKey = "ArmPosition";
+  public static final String kArmPKey = "ArmP";
+
   // The P gain for the PID controller that drives this arm.
-  private static final double kArmKp = 50.0;
+  private static double kArmKp = 50.0;
+
+  private static double armPositionDeg = 75.0;
 
   // distance per pulse = (angle per revolution) / (pulses per revolution)
   //  = (2 * PI rads) / (4096 pulses)
@@ -88,6 +94,14 @@ public class Robot extends TimedRobot {
     // Put Mechanism 2d to SmartDashboard
     SmartDashboard.putData("Arm Sim", m_mech2d);
     m_armTower.setColor(new Color8Bit(Color.kBlue));
+
+    // Set the Arm position setpoint and P constant to Preferences if the keys don't already exist
+    if (!Preferences.containsKey(kArmPositionKey)) {
+      Preferences.setDouble(kArmPositionKey, armPositionDeg);
+    }
+    if (!Preferences.containsKey(kArmPKey)) {
+      Preferences.setDouble(kArmPKey, kArmKp);
+    }
   }
 
   @Override
@@ -109,11 +123,22 @@ public class Robot extends TimedRobot {
     m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngleRads()));
   }
 
+  @Override
+  public void teleopInit() {
+    // Read Preferences for Arm setpoint and kP on entering Teleop
+    armPositionDeg = Preferences.getDouble(kArmPositionKey, armPositionDeg);
+    if (kArmKp != Preferences.getDouble(kArmPKey, kArmKp)) {
+      kArmKp = Preferences.getDouble(kArmPKey, kArmKp);
+      m_controller.setP(kArmKp);
+    }
+  }
+
   @Override
   public void teleopPeriodic() {
     if (m_joystick.getTrigger()) {
       // Here, we run PID control like normal, with a constant setpoint of 75 degrees.
-      var pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(75));
+      var pidOutput =
+          m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(armPositionDeg));
       m_motor.setVoltage(pidOutput);
     } else {
       // Otherwise, we disable the motor.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -701,7 +701,8 @@
       "Sensors",
       "Simulation",
       "Physics",
-      "Mechanism2d"
+      "Mechanism2d",
+      "Preferences"
     ],
     "foldername": "armsimulation",
     "gradlebase": "java",

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java

@@ -11,8 +11,8 @@ import edu.wpi.first.wpilibj.drive.MecanumDrive;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a sample program that uses mecanum drive with a gyro sensor to maintain rotation
- * vectorsin relation to the starting orientation of the robot (field-oriented controls).
+ * This is a sample program that uses mecanum drive with a gyro sensor to maintain rotation vectors
+ * in relation to the starting orientation of the robot (field-oriented controls).
  */
 public class Robot extends TimedRobot {
   // gyro calibration constant, may need to be adjusted;
@@ -51,6 +51,6 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopPeriodic() {
     m_robotDrive.driveCartesian(
-        m_joystick.getX(), m_joystick.getY(), m_joystick.getZ(), m_gyro.getAngle());
+        -m_joystick.getY(), m_joystick.getX(), m_joystick.getZ(), m_gyro.getAngle());
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java

@@ -42,6 +42,6 @@ public class Robot extends TimedRobot {
   public void teleopPeriodic() {
     // Use the joystick X axis for lateral movement, Y axis for forward
     // movement, and Z axis for rotation.
-    m_robotDrive.driveCartesian(m_stick.getX(), m_stick.getY(), m_stick.getZ(), 0.0);
+    m_robotDrive.driveCartesian(-m_stick.getY(), m_stick.getX(), m_stick.getZ(), 0.0);
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java

@@ -68,8 +68,8 @@ public class DriveSubsystem extends SubsystemBase {
     m_odometry.update(
         m_gyro.getRotation2d(),
         m_frontLeft.getState(),
-        m_rearLeft.getState(),
         m_frontRight.getState(),
+        m_rearLeft.getState(),
         m_rearRight.getState());
   }
 

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java

@@ -98,7 +98,7 @@ public class SwerveModule {
         m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
 
     // Calculate the turning motor output from the turning PID controller.
-    final var turnOutput =
+    final double turnOutput =
         m_turningPIDController.calculate(m_turningEncoder.get(), state.angle.getRadians());
 
     // Calculate the turning motor output from the turning PID controller.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java

@@ -92,4 +92,12 @@ public class Robot extends TimedRobot {
   /** This function is called periodically during test mode. */
   @Override
   public void testPeriodic() {}
+
+  /** This function is called once when the robot is first started up. */
+  @Override
+  public void simulationInit() {}
+
+  /** This function is called periodically whilst in simulation. */
+  @Override
+  public void simulationPeriodic() {}
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java

@@ -95,4 +95,12 @@ public class Robot extends TimedRobot {
   /** This function is called periodically during test mode. */
   @Override
   public void testPeriodic() {}
+
+  /** This function is called once when the robot is first started up. */
+  @Override
+  public void simulationInit() {}
+
+  /** This function is called periodically whilst in simulation. */
+  @Override
+  public void simulationPeriodic() {}
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java

@@ -46,4 +46,10 @@ public class Robot extends TimedRobot {
 
   @Override
   public void testPeriodic() {}
+
+  @Override
+  public void simulationInit() {}
+
+  @Override
+  public void simulationPeriodic() {}
 }

wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java

@@ -128,7 +128,7 @@ public class SimpleMotorFeedforward {
   }
 
   /**
-   * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
+   * Calculates the minimum achievable acceleration given a maximum voltage supply and a velocity.
    * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
    * simultaneously achievable - enter the velocity constraint, and this will give you a
    * simultaneously-achievable acceleration constraint.

wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java

@@ -89,6 +89,42 @@ public final class LinearSystemId {
         new Matrix<>(Nat.N1(), Nat.N1()));
   }
 
+  /**
+   * Create a state-space model of a DC motor system. The states of the system are [angular
+   * position, angular velocity], inputs are [voltage], and outputs are [angular position, angular
+   * velocity].
+   *
+   * @param motor The motor (or gearbox) attached to the DC motor.
+   * @param jKgMetersSquared The moment of inertia J of the DC motor.
+   * @param G The reduction between motor and drum, as a ratio of output to input.
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if jKgMetersSquared &lt;= 0 or G &lt;= 0.
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N2, N1, N2> createDCMotorSystem(
+      DCMotor motor, double jKgMetersSquared, double G) {
+    if (jKgMetersSquared <= 0.0) {
+      throw new IllegalArgumentException("J must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw new IllegalArgumentException("G must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2())
+            .fill(
+                0,
+                1,
+                0,
+                -G
+                    * G
+                    * motor.KtNMPerAmp
+                    / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)),
+        VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)),
+        Matrix.eye(Nat.N2()),
+        new Matrix<>(Nat.N2(), Nat.N1()));
+  }
+
   /**
    * Create a state-space model of a differential drive drivetrain. In this model, the states are
    * [v_left, v_right]ᵀ, inputs are [V_left, V_right]ᵀ and outputs are [v_left, v_right]ᵀ.

wpimath/src/main/native/include/frc/controller/ArmFeedforward.h

@@ -35,14 +35,14 @@ class WPILIB_DLLEXPORT ArmFeedforward {
    * Creates a new ArmFeedforward with the specified gains.
    *
    * @param kS   The static gain, in volts.
-   * @param kCos The gravity gain, in volts.
+   * @param kG The gravity gain, in volts.
    * @param kV   The velocity gain, in volt seconds per radian.
    * @param kA   The acceleration gain, in volt seconds^2 per radian.
    */
   constexpr ArmFeedforward(
-      units::volt_t kS, units::volt_t kCos, units::unit_t<kv_unit> kV,
+      units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
       units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
-      : kS(kS), kCos(kCos), kV(kV), kA(kA) {}
+      : kS(kS), kG(kG), kV(kV), kA(kA) {}
 
   /**
    * Calculates the feedforward from the gains and setpoints.
@@ -56,7 +56,7 @@ class WPILIB_DLLEXPORT ArmFeedforward {
                           units::unit_t<Velocity> velocity,
                           units::unit_t<Acceleration> acceleration =
                               units::unit_t<Acceleration>(0)) const {
-    return kS * wpi::sgn(velocity) + kCos * units::math::cos(angle) +
+    return kS * wpi::sgn(velocity) + kG * units::math::cos(angle) +
            kV * velocity + kA * acceleration;
   }
 
@@ -79,7 +79,7 @@ class WPILIB_DLLEXPORT ArmFeedforward {
       units::volt_t maxVoltage, units::unit_t<Angle> angle,
       units::unit_t<Acceleration> acceleration) {
     // Assume max velocity is positive
-    return (maxVoltage - kS - kCos * units::math::cos(angle) -
+    return (maxVoltage - kS - kG * units::math::cos(angle) -
             kA * acceleration) /
            kV;
   }
@@ -100,7 +100,7 @@ class WPILIB_DLLEXPORT ArmFeedforward {
       units::volt_t maxVoltage, units::unit_t<Angle> angle,
       units::unit_t<Acceleration> acceleration) {
     // Assume min velocity is negative, ks flips sign
-    return (-maxVoltage + kS - kCos * units::math::cos(angle) -
+    return (-maxVoltage + kS - kG * units::math::cos(angle) -
             kA * acceleration) /
            kV;
   }
@@ -121,7 +121,7 @@ class WPILIB_DLLEXPORT ArmFeedforward {
       units::volt_t maxVoltage, units::unit_t<Angle> angle,
       units::unit_t<Velocity> velocity) {
     return (maxVoltage - kS * wpi::sgn(velocity) -
-            kCos * units::math::cos(angle) - kV * velocity) /
+            kG * units::math::cos(angle) - kV * velocity) /
            kA;
   }
 
@@ -144,7 +144,7 @@ class WPILIB_DLLEXPORT ArmFeedforward {
   }
 
   units::volt_t kS{0};
-  units::volt_t kCos{0};
+  units::volt_t kG{0};
   units::unit_t<kv_unit> kV{0};
   units::unit_t<ka_unit> kA{0};
 };

wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h

@@ -6,7 +6,6 @@
 
 #include <wpi/SymbolExports.h>
 
-#include "DifferentialDriveKinematics.h"
 #include "frc/geometry/Pose2d.h"
 #include "units/length.h"
 

wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h

@@ -320,6 +320,38 @@ class WPILIB_DLLEXPORT LinearSystemId {
     return LinearSystem<1, 1, 1>(A, B, C, D);
   }
 
+  /**
+   * Constructs the state-space model for a DC motor motor.
+   *
+   * States: [[angular position, angular velocity]]
+   * Inputs: [[voltage]]
+   * Outputs: [[angular position, angular velocity]]
+   *
+   * @param motor Instance of DCMotor.
+   * @param J Moment of inertia.
+   * @param G Gear ratio from motor to carriage.
+   * @throws std::domain_error if J <= 0 or G <= 0.
+   */
+  static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor,
+                                             units::kilogram_square_meter_t J,
+                                             double G) {
+    if (J <= 0_kg_sq_m) {
+      throw std::domain_error("J must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw std::domain_error("G must be greater than zero.");
+    }
+
+    Eigen::Matrix<double, 2, 2> A{
+        {0.0, 1.0},
+        {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
+    Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
+    Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+    Eigen::Matrix<double, 2, 1> D{0.0, 0.0};
+
+    return LinearSystem<2, 1, 2>(A, B, C, D);
+  }
+
   /**
    * Constructs the state-space model for a drivetrain.
    *

wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java

@@ -64,6 +64,19 @@ class LinearSystemIDTest {
     assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
   }
 
+  @Test
+  void testDCMotorSystem() {
+    var model = LinearSystemId.createDCMotorSystem(DCMotor.getNEO(2), 0.00032, 1.0);
+    assertTrue(
+        model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -26.87032), 0.001));
+
+    assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1354.166667), 0.001));
+
+    assertTrue(model.getC().isEqual(Matrix.eye(Nat.N2()), 0.001));
+
+    assertTrue(model.getD().isEqual(new Matrix<>(Nat.N2(), Nat.N1()), 0.001));
+  }
+
   @Test
   void testIdentifyPositionSystem() {
     // By controls engineering in frc,

wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp

@@ -146,7 +146,7 @@ void AssertCentralResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
     // half the window size in the past.
     // The order of accuracy is O(h^(N - d)) where N is number of stencil
     // points and d is order of derivative
-    EXPECT_NEAR(dfdx((i - (Samples - 1) / 2) * h.value()),
+    EXPECT_NEAR(dfdx((i - static_cast<int>((Samples - 1) / 2)) * h.value()),
                 filter.Calculate(f(i * h.value())),
                 std::pow(h.value(), Samples - Derivative));
   }

wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp

@@ -49,6 +49,18 @@ TEST(LinearSystemIDTest, FlywheelSystem) {
   ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
 }
 
+TEST(LinearSystemIDTest, DCMotorSystem) {
+  auto model = frc::LinearSystemId::DCMotorSystem(frc::DCMotor::NEO(2),
+                                                  0.00032_kg_sq_m, 1.0);
+  ASSERT_TRUE(model.A().isApprox(
+      Eigen::Matrix<double, 2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
+  ASSERT_TRUE(
+      model.B().isApprox(Eigen::Matrix<double, 2, 1>{0, 1354.166667}, 0.001));
+  ASSERT_TRUE(model.C().isApprox(
+      Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
+  ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 0.0}, 0.001));
+}
+
 TEST(LinearSystemIDTest, IdentifyPositionSystem) {
   // By controls engineering in frc,
   // x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u

wpiutil/src/main/native/cpp/StackTraceWrap.cpp

@@ -0,0 +1,18 @@
+// 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 <atomic>
+
+#include "wpi/StackTrace.h"
+
+static std::atomic<std::string (*)(int offset)> gStackTraceImpl{
+    wpi::GetStackTraceDefault};
+
+std::string wpi::GetStackTrace(int offset) {
+  return (gStackTraceImpl.load())(offset);
+}
+
+void wpi::SetGetStackTraceImpl(std::string (*func)(int offset)) {
+  gStackTraceImpl = func ? func : wpi::GetStackTraceDefault;
+}

wpiutil/src/main/native/include/wpi/StackTrace.h

@@ -16,6 +16,20 @@ namespace wpi {
  */
 std::string GetStackTrace(int offset);
 
+/**
+ * The default implementation used for GetStackTrace().
+ *
+ * @param offset The number of symbols at the top of the stack to ignore
+ */
+std::string GetStackTraceDefault(int offset);
+
+/**
+ * Set the implementation used by GetStackTrace().
+ *
+ * @param func Function called by GetStackTrace().
+ */
+void SetGetStackTraceImpl(std::string (*func)(int offset));
+
 }  // namespace wpi
 
 #endif  // WPIUTIL_WPI_STACKTRACE_H_

wpiutil/src/main/native/unix/StackTrace.cpp

@@ -13,7 +13,7 @@
 
 namespace wpi {
 
-std::string GetStackTrace(int offset) {
+std::string GetStackTraceDefault(int offset) {
   void* stackTrace[128];
   int stackSize = backtrace(stackTrace, 128);
   char** mangledSymbols = backtrace_symbols(stackTrace, stackSize);

wpiutil/src/main/native/windows/StackTrace.cpp

@@ -32,7 +32,7 @@ void StackTraceWalker::OnOutput(LPCTSTR szText) {
 
 namespace wpi {
 
-std::string GetStackTrace(int offset) {
+std::string GetStackTraceDefault(int offset) {
   // TODO: implement offset
   std::string output;
   StackTraceWalker walker(output);