Add ArrayRef overloads to new command classes (#2216)

Also default requirements to {} in all cases for consistency.

.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
   "enableCppIntellisense": true,
   "currentLanguage": "cpp",
-  "projectYear": "Beta2020-2",
+  "projectYear": "2020",
   "teamNumber": 0
 }

CONTRIBUTING.md

@@ -37,7 +37,7 @@ So you want to contribute your changes back to WPILib. Great! We have a few cont
 
 ## Coding Guidelines
 
-WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system. We currently use clang-format 5.0 with wpiformat.
+WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system. We currently use clang-format 6.0 with wpiformat.
 
 While the library should be fully formatted according to the styles, additional elements of the style guide were not followed when the library was initially created. All new code should follow the guidelines. If you are looking for some easy ramp-up tasks, finding areas that don't follow the style guide and fixing them is very welcome.
 

FasterBuilds.md

@@ -0,0 +1,15 @@
+# Faster Builds for Developers
+
+When you run `./gradlew build`, it builds EVERYTHING. This means debug and release builds for desktop and all installed cross compilers. For many developers, this is way too much, and causes much developer pain.
+
+To help with some of these things, common tasks have shortcuts to only build necessary things for common development and testing tasks.
+
+## Development (Desktop)
+
+For projects `wpiutil`, `ntcore`, `cscore`, `hal` `wpilibOldCommands`, `wpilibNewCommands` and `cameraserver`, a `testDesktopJava` and a `testDesktopCpp` task exists. These can be ran with `./gradlew :projectName:task`, and will only build the minimum things required to run those tests.
+
+For `wpilibc`, a `testDesktopCpp` task exists. For `wpilibj`, a `testDesktopJava` task exists.
+
+For `wpilibcExamples`, a `buildDesktopCpp` task exists (These can't be ran, but they can compile).
+
+For `wpilibjExamples`, a `buildDesktopJava` task exists.

cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp

@@ -13,6 +13,7 @@
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
 #include <wpi/DenseMap.h>
+#include <wpi/ManagedStatic.h>
 #include <wpi/SmallString.h>
 #include <wpi/StringMap.h>
 #include <wpi/mutex.h>
@@ -47,8 +48,14 @@ struct CameraServer::Impl {
 };
 
 CameraServer* CameraServer::GetInstance() {
-  static CameraServer instance;
-  return &instance;
+  struct Creator {
+    static void* call() { return new CameraServer{}; }
+  };
+  struct Deleter {
+    static void call(void* ptr) { delete static_cast<CameraServer*>(ptr); }
+  };
+  static wpi::ManagedStatic<CameraServer, Creator, Deleter> instance;
+  return &(*instance);
 }
 
 static wpi::StringRef MakeSourceValue(CS_Source source,

cscore/src/main/native/cpp/HttpCameraImpl.cpp

@@ -412,7 +412,11 @@ std::unique_ptr<PropertyImpl> HttpCameraImpl::CreateEmptyProperty(
 }
 
 bool HttpCameraImpl::CacheProperties(CS_Status* status) const {
+#ifdef _MSC_VER  // work around VS2019 16.4.0 bug
+  std::scoped_lock<wpi::mutex> lock(m_mutex);
+#else
   std::scoped_lock lock(m_mutex);
+#endif
 
   // Pretty typical set of video modes
   m_videoModes.clear();

cscore/src/main/native/cpp/UnlimitedHandleResource.h

@@ -78,7 +78,11 @@ template <typename THandle, typename TStruct, int typeValue, typename TMutex>
 template <typename... Args>
 THandle UnlimitedHandleResource<THandle, TStruct, typeValue, TMutex>::Allocate(
     Args&&... args) {
+#ifdef _MSC_VER  // work around VS2019 16.4.0 bug
+  std::scoped_lock<TMutex> lock(m_handleMutex);
+#else
   std::scoped_lock sync(m_handleMutex);
+#endif
   size_t i;
   for (i = 0; i < m_structures.size(); i++) {
     if (m_structures[i] == nullptr) {

docs/build.gradle

@@ -125,7 +125,6 @@ task generateJavaDocs(type: Javadoc) {
     ext.entryPoint = "$destinationDir/index.html"
 
     if (JavaVersion.current().isJava11Compatible()) {
-        options.addBooleanOption('-no-module-directories', true)
         doLast {
             // This is a work-around for https://bugs.openjdk.java.net/browse/JDK-8211194. Can be removed once that issue is fixed on JDK's side
             // Since JDK 11, package-list is missing from javadoc output files and superseded by element-list file, but a lot of external tools still need it

hal/src/generate/Instances.txt

@@ -3,6 +3,7 @@ kLanguage_CPlusPlus = 2
 kLanguage_Java = 3
 kLanguage_Python = 4
 kLanguage_DotNet = 5
+kLanguage_Kotlin = 6
 kCANPlugin_BlackJagBridge = 1
 kCANPlugin_2CAN = 2
 kFramework_Iterative = 1
@@ -41,4 +42,11 @@ kDriverStationEIO_TouchSlider = 11
 kADXL345_SPI = 1
 kADXL345_I2C = 2
 kCommand_Scheduler = 1
+kCommand2_Scheduler = 2
 kSmartDashboard_Instance = 1
+kKinematics_DifferentialDrive = 1
+kKinematics_MecanumDrive = 2
+kKinematics_SwerveDrive = 3
+kOdometry_DifferentialDrive = 1
+kOdometry_MecanumDrive = 2
+kOdometry_SwerveDrive = 3

hal/src/main/java/edu/wpi/first/hal/SPIJNI.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -61,4 +61,6 @@ public class SPIJNI extends JNIWrapper {
                                                    double timeout);
 
   public static native int spiGetAutoDroppedCount(int port);
+
+  public static native void spiConfigureAutoStall(int port, int csToSclkTicks, int stallTicks, int pow2BytesPerRead);
 }

hal/src/main/java/edu/wpi/first/hal/sim/DriverStationSim.java

@@ -79,6 +79,15 @@ public class DriverStationSim {
     DriverStationDataJNI.notifyNewData();
   }
 
+  /**
+   * Toggles suppression of DriverStation.reportError and reportWarning messages.
+   *
+   * @param shouldSend If false then messages will will be suppressed.
+   */
+  public void setSendError(boolean shouldSend) {
+    DriverStationDataJNI.setSendError(shouldSend);
+  }
+
   public void resetData() {
     DriverStationDataJNI.resetData();
   }

hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DriverStationDataJNI.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -49,5 +49,7 @@ public class DriverStationDataJNI extends JNIWrapper {
   public static native void registerAllCallbacks(NotifyCallback callback, boolean initialNotify);
   public static native void notifyNewData();
 
+  public static native void setSendError(boolean shouldSend);
+
   public static native void resetData();
 }

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

@@ -9,6 +9,8 @@
 
 #include <cstring>
 
+#include <FRC_FPGA_ChipObject/fpgainterfacecapi/NiFpga_HMB.h>
+
 #include "ConstantsInternal.h"
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
@@ -19,14 +21,6 @@
 
 using namespace hal;
 
-extern "C" {
-NiFpga_Status NiFpga_ClientFunctionCall(NiFpga_Session session, uint32_t group,
-                                        uint32_t functionId,
-                                        const void* inBuffer,
-                                        size_t inBufferSize, void* outBuffer,
-                                        size_t outBufferSize);
-}  // extern "C"
-
 namespace {
 struct AddressableLED {
   std::unique_ptr<tLED> led;
@@ -52,43 +46,6 @@ void InitializeAddressableLED() {
 }  // namespace init
 }  // namespace hal
 
-// Shim for broken ChipObject function
-static const uint32_t clientFeature_hostMemoryBuffer = 0;
-static const uint32_t hostMemoryBufferFunction_open = 2;
-
-// Input arguments for HMB open
-struct AtomicHMBOpenInputs {
-  const char* memoryName;
-};
-
-// Output arguments for HMB open
-struct AtomicHMBOpenOutputs {
-  size_t size;
-  void* virtualAddress;
-};
-
-static NiFpga_Status OpenHostMemoryBuffer(NiFpga_Session session,
-                                          const char* memoryName,
-                                          void** virtualAddress, size_t* size) {
-  struct AtomicHMBOpenOutputs outputs;
-
-  struct AtomicHMBOpenInputs inputs;
-  inputs.memoryName = memoryName;
-
-  NiFpga_Status retval = NiFpga_ClientFunctionCall(
-      session, clientFeature_hostMemoryBuffer, hostMemoryBufferFunction_open,
-      &inputs, sizeof(struct AtomicHMBOpenInputs), &outputs,
-      sizeof(struct AtomicHMBOpenOutputs));
-  if (NiFpga_IsError(retval)) {
-    return retval;
-  }
-  *virtualAddress = outputs.virtualAddress;
-  if (size != NULL) {
-    *size = outputs.size;
-  }
-  return retval;
-}
-
 extern "C" {
 
 HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
@@ -146,8 +103,8 @@ HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
 
   uint32_t session = led->led->getSystemInterface()->getHandle();
 
-  *status = OpenHostMemoryBuffer(session, "HMB_0_LED", &led->ledBuffer,
-                                 &led->ledBufferSize);
+  *status = NiFpga_OpenHostMemoryBuffer(session, "HMB_0_LED", &led->ledBuffer,
+                                        &led->ledBufferSize);
 
   if (*status != 0) {
     addressableLEDHandles->Free(handle);

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

@@ -631,12 +631,21 @@ int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) {
   return spiSystem->readTransferSkippedFullCount(status);
 }
 
-// These 2 functions are so the new stall functionality
-// can be tested. How they're used is not very clear
-// but I want them to be testable so we can add an impl.
-// We will not be including these in the headers
-void* HAL_GetSPIDMAManager() { return spiAutoDMA.get(); }
+void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
+                               int32_t stallTicks, int32_t pow2BytesPerRead,
+                               int32_t* status) {
+  std::scoped_lock lock(spiAutoMutex);
+  // FPGA only has one auto SPI engine
+  if (port != spiAutoPort) {
+    *status = INCOMPATIBLE_STATE;
+    return;
+  }
 
-void* HAL_GetSPISystem() { return spiSystem.get(); }
+  tSPI::tStallConfig stallConfig;
+  stallConfig.CsToSclkTicks = static_cast<uint8_t>(csToSclkTicks);
+  stallConfig.StallTicks = static_cast<uint16_t>(stallTicks);
+  stallConfig.Pow2BytesPerRead = static_cast<uint8_t>(pow2BytesPerRead);
+  spiSystem->writeStallConfig(stallConfig, status);
+}
 
 }  // extern "C"

hal/src/main/native/cpp/jni/SPIJNI.cpp

@@ -394,4 +394,20 @@ Java_edu_wpi_first_hal_SPIJNI_spiGetAutoDroppedCount
   return retval;
 }
 
+/*
+ * Class:     edu_wpi_first_hal_SPIJNI
+ * Method:    spiConfigureAutoStall
+ * Signature: (IIII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SPIJNI_spiConfigureAutoStall
+  (JNIEnv* env, jclass, jint port, jint csToSclkTicks, jint stallTicks,
+   jint pow2BytesPerRead)
+{
+  int32_t status = 0;
+  HAL_ConfigureSPIAutoStall(static_cast<HAL_SPIPort>(port), csToSclkTicks,
+                            stallTicks, pow2BytesPerRead, &status);
+  CheckStatus(env, status);
+}
+
 }  // extern "C"

hal/src/main/native/include/hal/ChipObject.h

@@ -27,6 +27,7 @@
 #include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tDutyCycle.h>
 #include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tEncoder.h>
 #include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tGlobal.h>
+#include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tHMB.h>
 #include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tInterrupt.h>
 #include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tLED.h>
 #include <FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tPWM.h>

hal/src/main/native/include/hal/SPI.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -244,6 +244,19 @@ int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer,
  */
 int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status);
 
+/**
+ * Configure the Auto SPI Stall time between reads.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP.
+ * @param csToSclkTicks the number of ticks to wait before asserting the cs pin
+ * @param stallTicks the number of ticks to stall for
+ * @param pow2BytesPerRead the number of bytes to read before stalling
+ */
+void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
+                               int32_t stallTicks, int32_t pow2BytesPerRead,
+                               int32_t* status);
+
 #ifdef __cplusplus
 }  // extern "C"
 #endif

hal/src/main/native/include/mockdata/SimCallbackRegistry.h

@@ -74,7 +74,11 @@ class SimCallbackRegistry : public impl::SimCallbackRegistryBase {
 
   template <typename... U>
   void Invoke(U&&... u) const {
+#ifdef _MSC_VER  // work around VS2019 16.4.0 bug
+    std::scoped_lock<wpi::recursive_spinlock> lock(m_mutex);
+#else
     std::scoped_lock lock(m_mutex);
+#endif
     if (m_callbacks) {
       const char* name = GetName();
       for (auto&& cb : *m_callbacks)

hal/src/main/native/sim/SPI.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,6 +18,8 @@ void InitializeSPI() {}
 }  // namespace init
 }  // namespace hal
 
+extern "C" {
+
 void HAL_InitializeSPI(HAL_SPIPort port, int32_t* status) {
   hal::init::CheckInit();
   SimSPIData[port].initialized = true;
@@ -63,3 +65,9 @@ int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer,
 int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) {
   return 0;
 }
+
+void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
+                               int32_t stallTicks, int32_t pow2BytesPerRead,
+                               int32_t* status) {}
+
+}  // extern "C"

hal/src/main/native/sim/jni/DriverStationDataJNI.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,6 +14,7 @@
 #include "CallbackStore.h"
 #include "edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI.h"
 #include "mockdata/DriverStationData.h"
+#include "mockdata/MockHooks.h"
 
 using namespace wpi::java;
 
@@ -446,6 +447,25 @@ Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_notifyNewData
   HALSIM_NotifyDriverStationNewData();
 }
 
+/*
+ * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method:    setSendError
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setSendError
+  (JNIEnv*, jclass, jboolean shouldSend)
+{
+  if (shouldSend) {
+    HALSIM_SetSendError(nullptr);
+  } else {
+    HALSIM_SetSendError([](HAL_Bool isError, int32_t errorCode,
+                           HAL_Bool isLVCode, const char* details,
+                           const char* location, const char* callStack,
+                           HAL_Bool printMsg) { return 1; });
+  }
+}
+
 /*
  * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
  * Method:    resetData

ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableInstance.java

@@ -44,6 +44,7 @@ public final class NetworkTableInstance implements AutoCloseable {
   public static final int kNetModeClient = 0x02;
   public static final int kNetModeStarting = 0x04;
   public static final int kNetModeFailure = 0x08;
+  public static final int kNetModeLocal = 0x10;
 
   /**
    * The default port that network tables operates on.
@@ -675,6 +676,23 @@ public final class NetworkTableInstance implements AutoCloseable {
     return NetworkTablesJNI.getNetworkMode(m_handle);
   }
 
+  /**
+   * Starts local-only operation.  Prevents calls to startServer or startClient
+   * from taking effect.  Has no effect if startServer or startClient
+   * has already been called.
+   */
+  public void startLocal() {
+    NetworkTablesJNI.startLocal(m_handle);
+  }
+
+  /**
+   * Stops local-only operation.  startServer or startClient can be called after
+   * this call to start a server or client.
+   */
+  public void stopLocal() {
+    NetworkTablesJNI.stopLocal(m_handle);
+  }
+
   /**
    * Starts a server using the networktables.ini as the persistent file,
    * using the default listening address and port.

ntcore/src/main/java/edu/wpi/first/networktables/NetworkTablesJNI.java

@@ -139,6 +139,8 @@ public final class NetworkTablesJNI {
 
   public static native void setNetworkIdentity(int inst, String name);
   public static native int getNetworkMode(int inst);
+  public static native void startLocal(int inst);
+  public static native void stopLocal(int inst);
   public static native void startServer(int inst, String persistFilename, String listenAddress, int port);
   public static native void stopServer(int inst);
   public static native void startClient(int inst);

ntcore/src/main/native/cpp/Dispatcher.cpp

@@ -115,6 +115,16 @@ DispatcherBase::~DispatcherBase() { Stop(); }
 
 unsigned int DispatcherBase::GetNetworkMode() const { return m_networkMode; }
 
+void DispatcherBase::StartLocal() {
+  {
+    std::scoped_lock lock(m_user_mutex);
+    if (m_active) return;
+    m_active = true;
+  }
+  m_networkMode = NT_NET_MODE_LOCAL;
+  m_storage.SetDispatcher(this, false);
+}
+
 void DispatcherBase::StartServer(
     const Twine& persist_filename,
     std::unique_ptr<wpi::NetworkAcceptor> acceptor) {

ntcore/src/main/native/cpp/Dispatcher.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -48,6 +48,7 @@ class DispatcherBase : public IDispatcher {
   virtual ~DispatcherBase();
 
   unsigned int GetNetworkMode() const;
+  void StartLocal();
   void StartServer(const Twine& persist_filename,
                    std::unique_ptr<wpi::NetworkAcceptor> acceptor);
   void StartClient();

ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -1368,6 +1368,30 @@ Java_edu_wpi_first_networktables_NetworkTablesJNI_getNetworkMode
   return nt::GetNetworkMode(inst);
 }
 
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    startLocal
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_startLocal
+  (JNIEnv*, jclass, jint inst)
+{
+  nt::StartLocal(inst);
+}
+
+/*
+ * Class:     edu_wpi_first_networktables_NetworkTablesJNI
+ * Method:    stopLocal
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_networktables_NetworkTablesJNI_stopLocal
+  (JNIEnv*, jclass, jint inst)
+{
+  nt::StopLocal(inst);
+}
+
 /*
  * Class:     edu_wpi_first_networktables_NetworkTablesJNI
  * Method:    startServer

ntcore/src/main/native/cpp/ntcore_c.cpp

@@ -539,6 +539,10 @@ unsigned int NT_GetNetworkMode(NT_Inst inst) {
   return nt::GetNetworkMode(inst);
 }
 
+void NT_StartLocal(NT_Inst inst) { nt::StartLocal(inst); }
+
+void NT_StopLocal(NT_Inst inst) { nt::StopLocal(inst); }
+
 void NT_StartServer(NT_Inst inst, const char* persist_filename,
                     const char* listen_address, unsigned int port) {
   nt::StartServer(inst, persist_filename, listen_address, port);

ntcore/src/main/native/cpp/ntcore_cpp.cpp

@@ -742,6 +742,20 @@ unsigned int GetNetworkMode(NT_Inst inst) {
   return ii->dispatcher.GetNetworkMode();
 }
 
+void StartLocal(NT_Inst inst) {
+  auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
+  if (!ii) return;
+
+  ii->dispatcher.StartLocal();
+}
+
+void StopLocal(NT_Inst inst) {
+  auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
+  if (!ii) return;
+
+  ii->dispatcher.Stop();
+}
+
 void StartServer(StringRef persist_filename, const char* listen_address,
                  unsigned int port) {
   auto ii = InstanceImpl::GetDefault();

ntcore/src/main/native/include/networktables/NetworkTableInstance.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -61,7 +61,8 @@ class NetworkTableInstance final {
     kNetModeServer = NT_NET_MODE_SERVER,
     kNetModeClient = NT_NET_MODE_CLIENT,
     kNetModeStarting = NT_NET_MODE_STARTING,
-    kNetModeFailure = NT_NET_MODE_FAILURE
+    kNetModeFailure = NT_NET_MODE_FAILURE,
+    kNetModeLocal = NT_NET_MODE_LOCAL
   };
 
   /**
@@ -298,6 +299,19 @@ class NetworkTableInstance final {
    */
   unsigned int GetNetworkMode() const;
 
+  /**
+   * Starts local-only operation.  Prevents calls to StartServer or StartClient
+   * from taking effect.  Has no effect if StartServer or StartClient
+   * has already been called.
+   */
+  void StartLocal();
+
+  /**
+   * Stops local-only operation.  StartServer or StartClient can be called after
+   * this call to start a server or client.
+   */
+  void StopLocal();
+
   /**
    * Starts a server using the specified filename, listening address, and port.
    *

ntcore/src/main/native/include/networktables/NetworkTableInstance.inl

@@ -81,6 +81,10 @@ inline unsigned int NetworkTableInstance::GetNetworkMode() const {
   return ::nt::GetNetworkMode(m_handle);
 }
 
+inline void NetworkTableInstance::StartLocal() { ::nt::StartLocal(m_handle); }
+
+inline void NetworkTableInstance::StopLocal() { ::nt::StopLocal(m_handle); }
+
 inline void NetworkTableInstance::StartServer(const Twine& persist_filename,
                                               const char* listen_address,
                                               unsigned int port) {

ntcore/src/main/native/include/ntcore_c.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -95,6 +95,7 @@ enum NT_NetworkMode {
   NT_NET_MODE_CLIENT = 0x02,   /* running in client mode */
   NT_NET_MODE_STARTING = 0x04, /* flag for starting (either client or server) */
   NT_NET_MODE_FAILURE = 0x08,  /* flag for failure (either client or server) */
+  NT_NET_MODE_LOCAL = 0x10,    /* running in local-only mode */
 };
 
 /*
@@ -1037,6 +1038,19 @@ void NT_SetNetworkIdentity(NT_Inst inst, const char* name, size_t name_len);
  */
 unsigned int NT_GetNetworkMode(NT_Inst inst);
 
+/**
+ * Starts local-only operation.  Prevents calls to NT_StartServer or
+ * NT_StartClient from taking effect.  Has no effect if NT_StartServer or
+ * NT_StartClient has already been called.
+ */
+void NT_StartLocal(NT_Inst inst);
+
+/**
+ * Stops local-only operation.  NT_StartServer or NT_StartClient can be called
+ * after this call to start a server or client.
+ */
+void NT_StopLocal(NT_Inst inst);
+
 /**
  * Starts a server using the specified filename, listening address, and port.
  *

ntcore/src/main/native/include/ntcore_cpp.h

@@ -1130,6 +1130,19 @@ unsigned int GetNetworkMode();
  */
 unsigned int GetNetworkMode(NT_Inst inst);
 
+/**
+ * Starts local-only operation.  Prevents calls to StartServer or StartClient
+ * from taking effect.  Has no effect if StartServer or StartClient
+ * has already been called.
+ */
+void StartLocal(NT_Inst inst);
+
+/**
+ * Stops local-only operation.  StartServer or StartClient can be called after
+ * this call to start a server or client.
+ */
+void StopLocal(NT_Inst inst);
+
 /**
  * Starts a server using the specified filename, listening address, and port.
  *

shared/config.gradle

@@ -8,9 +8,9 @@ nativeUtils {
   wpi {
     configureDependencies {
       wpiVersion = "-1"
-      niLibVersion = "2020.9.1"
+      niLibVersion = "2020.10.1"
       opencvVersion = "3.4.7-2"
-      googleTestVersion = "1.9.0-3-437e100"
+      googleTestVersion = "1.9.0-4-437e100-1"
       imguiVersion = "1.72b-2"
     }
   }

shared/cppDesktopTestTask.gradle

@@ -0,0 +1,21 @@
+model {
+    tasks {
+        def ts = $.testSuites
+        project.tasks.register('testDesktopCpp') { testTask->
+            def systemArch = getCurrentArch()
+            def found = false
+            ts.each {
+                if (it in GoogleTestTestSuiteSpec && it.name == "${nativeName}Test") {
+                    it.binaries.each {
+                        if (found) return
+                        def arch = it.targetPlatform.name
+                        if (arch == systemArch && it.buildType.name == 'debug') {
+                            testTask.dependsOn it.tasks.run
+                            found = true
+                        }
+                    }
+                }
+            }
+        }
+    }
+}

shared/examplecheck.gradle

@@ -70,6 +70,7 @@ task checkCommands(type: Task) {
             assert it.tags != null
             assert it.foldername != null
             assert it.replacename != null
+            assert it.commandversion != null
             if (project.isCppCommands) {
                 assert it.headers != null
                 assert !it.headers.isEmpty()

shared/javaDesktopTestTask.gradle

@@ -0,0 +1,3 @@
+tasks.register('testDesktopJava') {
+  dependsOn test
+}

shared/javacpp/setupBuild.gradle

@@ -141,6 +141,9 @@ model {
     }
 }
 
+apply from: "${rootDir}/shared/cppDesktopTestTask.gradle"
+apply from: "${rootDir}/shared/javaDesktopTestTask.gradle"
+
 tasks.withType(RunTestExecutable) {
     args "--gtest_output=xml:test_detail.xml"
     outputs.dir outputDir

shared/jni/setupBuild.gradle

@@ -275,6 +275,9 @@ model {
     }
 }
 
+apply from: "${rootDir}/shared/cppDesktopTestTask.gradle"
+apply from: "${rootDir}/shared/javaDesktopTestTask.gradle"
+
 ext.getJniSpecClass = {
     return JniNativeLibrarySpec
 }

simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.cpp

@@ -24,9 +24,18 @@ static void DisplayAnalogInputs() {
   bool hasInputs = false;
   static int numAnalog = HAL_GetNumAnalogInputs();
   static int numAccum = HAL_GetNumAccumulators();
+  bool first = true;
   for (int i = 0; i < numAnalog; ++i) {
     if (HALSIM_GetAnalogInInitialized(i)) {
       hasInputs = true;
+
+      if (!first) {
+        ImGui::Spacing();
+        ImGui::Spacing();
+      } else {
+        first = false;
+      }
+
       char name[32];
       std::snprintf(name, sizeof(name), "In[%d]", i);
       if (i < numAccum && HALSIM_GetAnalogGyroInitialized(i)) {

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java

@@ -89,7 +89,7 @@ public final class CommandScheduler implements Sendable {
 
 
   CommandScheduler() {
-    HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
+    HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand2_Scheduler);
     SendableRegistry.addLW(this, "Scheduler");
   }
 

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java

@@ -25,10 +25,20 @@ public abstract class PIDSubsystem extends SubsystemBase {
    * Creates a new PIDSubsystem.
    *
    * @param controller the PIDController to use
+   * @param initialPosition the initial setpoint of the subsystem
+   */
+  public PIDSubsystem(PIDController controller, double initialPosition) {
+    setSetpoint(initialPosition);
+    m_controller = requireNonNullParam(controller, "controller", "PIDSubsystem");
+  }
+
+  /**
+   * Creates a new PIDSubsystem.  Initial setpoint is zero.
+   *
+   * @param controller the PIDController to use
    */
   public PIDSubsystem(PIDController controller) {
-    requireNonNullParam(controller, "controller", "PIDSubsystem");
-    m_controller = controller;
+    this(controller, 0);
   }
 
   @Override

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -113,7 +113,7 @@ public class ProfiledPIDCommand extends CommandBase {
 
   @Override
   public void initialize() {
-    m_controller.reset();
+    m_controller.reset(m_measurement.getAsDouble());
   }
 
   @Override

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -27,10 +27,21 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
    * Creates a new ProfiledPIDSubsystem.
    *
    * @param controller the ProfiledPIDController to use
+   * @param initialPosition the initial goal position of the controller
+   */
+  public ProfiledPIDSubsystem(ProfiledPIDController controller,
+                              double initialPosition) {
+    m_controller = requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
+    setGoal(initialPosition);
+  }
+
+  /**
+   * Creates a new ProfiledPIDSubsystem.  Initial goal position is zero.
+   *
+   * @param controller the ProfiledPIDController to use
    */
   public ProfiledPIDSubsystem(ProfiledPIDController controller) {
-    requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
-    m_controller = controller;
+    this(controller, 0);
   }
 
   @Override
@@ -82,7 +93,7 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
    */
   public void enable() {
     m_enabled = true;
-    m_controller.reset();
+    m_controller.reset(getMeasurement());
   }
 
   /**

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java

@@ -76,7 +76,6 @@ public abstract class SubsystemBase implements Subsystem, Sendable {
    */
   public void addChild(String name, Sendable child) {
     SendableRegistry.addLW(child, getSubsystem(), name);
-    SendableRegistry.addChild(this, child);
   }
 
   @Override

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java

@@ -9,6 +9,8 @@ package edu.wpi.first.wpilibj2.command;
 
 import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
 
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
 /**
  * A subsystem that generates and runs trapezoidal motion profiles automatically.  The user
  * specifies how to use the current state of the motion profile by overriding the `useState` method.
@@ -20,41 +22,53 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
   private TrapezoidProfile.State m_state;
   private TrapezoidProfile.State m_goal;
 
-  /**
-   * Creates a new TrapezoidProfileSubsystem.
-   *
-   * @param constraints     The constraints (maximum velocity and acceleration) for the profiles.
-   * @param initialPosition The initial position of the controller mechanism when the subsystem
-   *                        is constructed.
-   */
-  public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
-                                   double initialPosition) {
-    m_constraints = constraints;
-    m_state = new TrapezoidProfile.State(initialPosition, 0);
-    m_period = 0.02;
-  }
+  private boolean m_enabled = true;
 
   /**
    * Creates a new TrapezoidProfileSubsystem.
    *
    * @param constraints     The constraints (maximum velocity and acceleration) for the profiles.
-   * @param initialPosition The initial position of the controller mechanism when the subsystem
+   * @param initialPosition The initial position of the controlled mechanism when the subsystem
    *                        is constructed.
    * @param period          The period of the main robot loop, in seconds.
    */
   public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
                                    double initialPosition,
                                    double period) {
-    m_constraints = constraints;
+    m_constraints = requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem");
     m_state = new TrapezoidProfile.State(initialPosition, 0);
+    setGoal(initialPosition);
     m_period = period;
   }
 
+  /**
+   * Creates a new TrapezoidProfileSubsystem.
+   *
+   * @param constraints     The constraints (maximum velocity and acceleration) for the profiles.
+   * @param initialPosition The initial position of the controlled mechanism when the subsystem
+   *                        is constructed.
+   */
+  public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
+                                   double initialPosition) {
+    this(constraints, initialPosition, 0.02);
+  }
+
+  /**
+   * Creates a new TrapezoidProfileSubsystem.
+   *
+   * @param constraints     The constraints (maximum velocity and acceleration) for the profiles.
+   */
+  public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints) {
+    this(constraints, 0, 0.02);
+  }
+
   @Override
   public void periodic() {
     var profile = new TrapezoidProfile(m_constraints, m_goal, m_state);
     m_state = profile.calculate(m_period);
-    useState(m_state);
+    if (m_enabled) {
+      useState(m_state);
+    }
   }
 
   /**
@@ -75,6 +89,20 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
     setGoal(new TrapezoidProfile.State(goal, 0));
   }
 
+  /**
+   * Enable the TrapezoidProfileSubsystem's output.
+   */
+  public void enable() {
+    m_enabled = true;
+  }
+
+  /**
+   * Disable the TrapezoidProfileSubsystem's output.
+   */
+  public void disable() {
+    m_enabled = false;
+  }
+
   /**
    * Users should override this to consume the current state of the motion profile.
    *

wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -51,6 +51,13 @@ ParallelRaceGroup Command::WithInterrupt(std::function<bool()> condition) && {
 SequentialCommandGroup Command::BeforeStarting(
     std::function<void()> toRun,
     std::initializer_list<Subsystem*> requirements) && {
+  return std::move(*this).BeforeStarting(
+      std::move(toRun),
+      wpi::makeArrayRef(requirements.begin(), requirements.end()));
+}
+
+SequentialCommandGroup Command::BeforeStarting(
+    std::function<void()> toRun, wpi::ArrayRef<Subsystem*> requirements) && {
   std::vector<std::unique_ptr<Command>> temp;
   temp.emplace_back(
       std::make_unique<InstantCommand>(std::move(toRun), requirements));
@@ -61,6 +68,13 @@ SequentialCommandGroup Command::BeforeStarting(
 SequentialCommandGroup Command::AndThen(
     std::function<void()> toRun,
     std::initializer_list<Subsystem*> requirements) && {
+  return std::move(*this).AndThen(
+      std::move(toRun),
+      wpi::makeArrayRef(requirements.begin(), requirements.end()));
+}
+
+SequentialCommandGroup Command::AndThen(
+    std::function<void()> toRun, wpi::ArrayRef<Subsystem*> requirements) && {
   std::vector<std::unique_ptr<Command>> temp;
   temp.emplace_back(std::move(*this).TransferOwnership());
   temp.emplace_back(

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,6 +21,10 @@ void CommandBase::AddRequirements(
   m_requirements.insert(requirements.begin(), requirements.end());
 }
 
+void CommandBase::AddRequirements(wpi::ArrayRef<Subsystem*> requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
 void CommandBase::AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements) {
   m_requirements.insert(requirements.begin(), requirements.end());
 }

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,6 +11,7 @@
 #include <frc/WPIErrors.h>
 #include <frc/smartdashboard/SendableBuilder.h>
 #include <frc/smartdashboard/SendableRegistry.h>
+#include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <networktables/NetworkTableEntry.h>
 #include <wpi/DenseMap.h>
@@ -67,6 +68,8 @@ static bool ContainsKey(const TMap& map, TKey keyToCheck) {
 }
 
 CommandScheduler::CommandScheduler() : m_impl(new Impl) {
+  HAL_Report(HALUsageReporting::kResourceType_Command,
+             HALUsageReporting::kCommand2_Scheduler);
   frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
 }
 
@@ -245,6 +248,12 @@ void CommandScheduler::RegisterSubsystem(
   }
 }
 
+void CommandScheduler::RegisterSubsystem(wpi::ArrayRef<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    RegisterSubsystem(subsystem);
+  }
+}
+
 void CommandScheduler::UnregisterSubsystem(
     std::initializer_list<Subsystem*> subsystems) {
   for (auto* subsystem : subsystems) {
@@ -252,6 +261,13 @@ void CommandScheduler::UnregisterSubsystem(
   }
 }
 
+void CommandScheduler::UnregisterSubsystem(
+    wpi::ArrayRef<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    UnregisterSubsystem(subsystem);
+  }
+}
+
 Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
   auto&& find = m_impl->subsystems.find(subsystem);
   if (find != m_impl->subsystems.end()) {

wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -20,6 +20,18 @@ FunctionalCommand::FunctionalCommand(
   AddRequirements(requirements);
 }
 
+FunctionalCommand::FunctionalCommand(std::function<void()> onInit,
+                                     std::function<void()> onExecute,
+                                     std::function<void(bool)> onEnd,
+                                     std::function<bool()> isFinished,
+                                     wpi::ArrayRef<Subsystem*> requirements)
+    : m_onInit{std::move(onInit)},
+      m_onExecute{std::move(onExecute)},
+      m_onEnd{std::move(onEnd)},
+      m_isFinished{std::move(isFinished)} {
+  AddRequirements(requirements);
+}
+
 void FunctionalCommand::Initialize() { m_onInit(); }
 
 void FunctionalCommand::Execute() { m_onExecute(); }

wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,6 +15,12 @@ InstantCommand::InstantCommand(std::function<void()> toRun,
   AddRequirements(requirements);
 }
 
+InstantCommand::InstantCommand(std::function<void()> toRun,
+                               wpi::ArrayRef<Subsystem*> requirements)
+    : m_toRun{std::move(toRun)} {
+  AddRequirements(requirements);
+}
+
 InstantCommand::InstantCommand() : m_toRun{[] {}} {}
 
 void InstantCommand::Initialize() { m_toRun(); }

wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -50,6 +50,46 @@ MecanumControllerCommand::MecanumControllerCommand(
   AddRequirements(requirements);
 }
 
+MecanumControllerCommand::MecanumControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SimpleMotorFeedforward<units::meters> feedforward,
+    frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+    frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    units::meters_per_second_t maxWheelVelocity,
+    std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+    frc2::PIDController frontLeftController,
+    frc2::PIDController rearLeftController,
+    frc2::PIDController frontRightController,
+    frc2::PIDController rearRightController,
+    std::function<void(units::volt_t, units::volt_t, units::volt_t,
+                       units::volt_t)>
+        output,
+    wpi::ArrayRef<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_feedforward(feedforward),
+      m_kinematics(kinematics),
+      m_xController(std::make_unique<frc2::PIDController>(xController)),
+      m_yController(std::make_unique<frc2::PIDController>(yController)),
+      m_thetaController(
+          std::make_unique<frc::ProfiledPIDController<units::radians>>(
+              thetaController)),
+      m_maxWheelVelocity(maxWheelVelocity),
+      m_frontLeftController(
+          std::make_unique<frc2::PIDController>(frontLeftController)),
+      m_rearLeftController(
+          std::make_unique<frc2::PIDController>(rearLeftController)),
+      m_frontRightController(
+          std::make_unique<frc2::PIDController>(frontRightController)),
+      m_rearRightController(
+          std::make_unique<frc2::PIDController>(rearRightController)),
+      m_currentWheelSpeeds(currentWheelSpeeds),
+      m_outputVolts(output),
+      m_usePID(true) {
+  AddRequirements(requirements);
+}
+
 MecanumControllerCommand::MecanumControllerCommand(
     frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
     frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
@@ -74,6 +114,30 @@ MecanumControllerCommand::MecanumControllerCommand(
   AddRequirements(requirements);
 }
 
+MecanumControllerCommand::MecanumControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+    frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    units::meters_per_second_t maxWheelVelocity,
+    std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+                       units::meters_per_second_t, units::meters_per_second_t)>
+        output,
+    wpi::ArrayRef<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_kinematics(kinematics),
+      m_xController(std::make_unique<frc2::PIDController>(xController)),
+      m_yController(std::make_unique<frc2::PIDController>(yController)),
+      m_thetaController(
+          std::make_unique<frc::ProfiledPIDController<units::radians>>(
+              thetaController)),
+      m_maxWheelVelocity(maxWheelVelocity),
+      m_outputVel(output),
+      m_usePID(false) {
+  AddRequirements(requirements);
+}
+
 void MecanumControllerCommand::Initialize() {
   m_prevTime = 0_s;
   auto initialState = m_trajectory.Sample(0_s);

wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,6 +16,13 @@ NotifierCommand::NotifierCommand(std::function<void()> toRun,
   AddRequirements(requirements);
 }
 
+NotifierCommand::NotifierCommand(std::function<void()> toRun,
+                                 units::second_t period,
+                                 wpi::ArrayRef<Subsystem*> requirements)
+    : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} {
+  AddRequirements(requirements);
+}
+
 NotifierCommand::NotifierCommand(NotifierCommand&& other)
     : CommandHelper(std::move(other)),
       m_toRun(other.m_toRun),

wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,6 +21,18 @@ PIDCommand::PIDCommand(PIDController controller,
   AddRequirements(requirements);
 }
 
+PIDCommand::PIDCommand(PIDController controller,
+                       std::function<double()> measurementSource,
+                       std::function<double()> setpointSource,
+                       std::function<void(double)> useOutput,
+                       wpi::ArrayRef<Subsystem*> requirements)
+    : m_controller{controller},
+      m_measurement{std::move(measurementSource)},
+      m_setpoint{std::move(setpointSource)},
+      m_useOutput{std::move(useOutput)} {
+  AddRequirements(requirements);
+}
+
 PIDCommand::PIDCommand(PIDController controller,
                        std::function<double()> measurementSource,
                        double setpoint, std::function<void(double)> useOutput,
@@ -28,6 +40,13 @@ PIDCommand::PIDCommand(PIDController controller,
     : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; },
                  useOutput, requirements) {}
 
+PIDCommand::PIDCommand(PIDController controller,
+                       std::function<double()> measurementSource,
+                       double setpoint, std::function<void(double)> useOutput,
+                       wpi::ArrayRef<Subsystem*> requirements)
+    : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; },
+                 useOutput, requirements) {}
+
 void PIDCommand::Initialize() { m_controller.Reset(); }
 
 void PIDCommand::Execute() {

wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp

@@ -9,8 +9,10 @@
 
 using namespace frc2;
 
-PIDSubsystem::PIDSubsystem(PIDController controller)
-    : m_controller{controller} {}
+PIDSubsystem::PIDSubsystem(PIDController controller, double initialPosition)
+    : m_controller{controller} {
+  SetSetpoint(initialPosition);
+}
 
 void PIDSubsystem::Periodic() {
   if (m_enabled) {

wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -32,6 +32,28 @@ RamseteCommand::RamseteCommand(
   AddRequirements(requirements);
 }
 
+RamseteCommand::RamseteCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::RamseteController controller,
+    frc::SimpleMotorFeedforward<units::meters> feedforward,
+    frc::DifferentialDriveKinematics kinematics,
+    std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
+    frc2::PIDController leftController, frc2::PIDController rightController,
+    std::function<void(volt_t, volt_t)> output,
+    wpi::ArrayRef<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_controller(controller),
+      m_feedforward(feedforward),
+      m_kinematics(kinematics),
+      m_speeds(wheelSpeeds),
+      m_leftController(std::make_unique<frc2::PIDController>(leftController)),
+      m_rightController(std::make_unique<frc2::PIDController>(rightController)),
+      m_outputVolts(output),
+      m_usePID(true) {
+  AddRequirements(requirements);
+}
+
 RamseteCommand::RamseteCommand(
     frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
     frc::RamseteController controller,
@@ -48,6 +70,22 @@ RamseteCommand::RamseteCommand(
   AddRequirements(requirements);
 }
 
+RamseteCommand::RamseteCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::RamseteController controller,
+    frc::DifferentialDriveKinematics kinematics,
+    std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
+        output,
+    wpi::ArrayRef<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_controller(controller),
+      m_kinematics(kinematics),
+      m_outputVel(output),
+      m_usePID(false) {
+  AddRequirements(requirements);
+}
+
 void RamseteCommand::Initialize() {
   m_prevTime = 0_s;
   auto initialState = m_trajectory.Sample(0_s);

wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,4 +15,10 @@ RunCommand::RunCommand(std::function<void()> toRun,
   AddRequirements(requirements);
 }
 
+RunCommand::RunCommand(std::function<void()> toRun,
+                       wpi::ArrayRef<Subsystem*> requirements)
+    : m_toRun{std::move(toRun)} {
+  AddRequirements(requirements);
+}
+
 void RunCommand::Execute() { m_toRun(); }

wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,6 +16,13 @@ StartEndCommand::StartEndCommand(std::function<void()> onInit,
   AddRequirements(requirements);
 }
 
+StartEndCommand::StartEndCommand(std::function<void()> onInit,
+                                 std::function<void()> onEnd,
+                                 wpi::ArrayRef<Subsystem*> requirements)
+    : m_onInit{std::move(onInit)}, m_onEnd{std::move(onEnd)} {
+  AddRequirements(requirements);
+}
+
 StartEndCommand::StartEndCommand(const StartEndCommand& other)
     : CommandHelper(other) {
   m_onInit = other.m_onInit;

wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp

@@ -63,5 +63,4 @@ void SubsystemBase::SetSubsystem(const wpi::Twine& name) {
 void SubsystemBase::AddChild(std::string name, frc::Sendable* child) {
   auto& registry = frc::SendableRegistry::GetInstance();
   registry.AddLW(child, GetSubsystem(), name);
-  registry.AddChild(this, child);
 }

wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -22,6 +22,12 @@ Button Button::WhenPressed(std::function<void()> toRun,
   return *this;
 }
 
+Button Button::WhenPressed(std::function<void()> toRun,
+                           wpi::ArrayRef<Subsystem*> requirements) {
+  WhenActive(std::move(toRun), requirements);
+  return *this;
+}
+
 Button Button::WhileHeld(Command* command, bool interruptible) {
   WhileActiveContinous(command, interruptible);
   return *this;
@@ -33,6 +39,12 @@ Button Button::WhileHeld(std::function<void()> toRun,
   return *this;
 }
 
+Button Button::WhileHeld(std::function<void()> toRun,
+                         wpi::ArrayRef<Subsystem*> requirements) {
+  WhileActiveContinous(std::move(toRun), requirements);
+  return *this;
+}
+
 Button Button::WhenHeld(Command* command, bool interruptible) {
   WhileActiveOnce(command, interruptible);
   return *this;
@@ -49,6 +61,12 @@ Button Button::WhenReleased(std::function<void()> toRun,
   return *this;
 }
 
+Button Button::WhenReleased(std::function<void()> toRun,
+                            wpi::ArrayRef<Subsystem*> requirements) {
+  WhenInactive(std::move(toRun), requirements);
+  return *this;
+}
+
 Button Button::ToggleWhenPressed(Command* command, bool interruptible) {
   ToggleWhenActive(command, interruptible);
   return *this;

wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -30,6 +30,12 @@ Trigger Trigger::WhenActive(Command* command, bool interruptible) {
 
 Trigger Trigger::WhenActive(std::function<void()> toRun,
                             std::initializer_list<Subsystem*> requirements) {
+  return WhenActive(std::move(toRun), wpi::makeArrayRef(requirements.begin(),
+                                                        requirements.end()));
+}
+
+Trigger Trigger::WhenActive(std::function<void()> toRun,
+                            wpi::ArrayRef<Subsystem*> requirements) {
   return WhenActive(InstantCommand(std::move(toRun), requirements));
 }
 
@@ -52,6 +58,13 @@ Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
 Trigger Trigger::WhileActiveContinous(
     std::function<void()> toRun,
     std::initializer_list<Subsystem*> requirements) {
+  return WhileActiveContinous(
+      std::move(toRun),
+      wpi::makeArrayRef(requirements.begin(), requirements.end()));
+}
+
+Trigger Trigger::WhileActiveContinous(std::function<void()> toRun,
+                                      wpi::ArrayRef<Subsystem*> requirements) {
   return WhileActiveContinous(InstantCommand(std::move(toRun), requirements));
 }
 
@@ -87,6 +100,12 @@ Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
 
 Trigger Trigger::WhenInactive(std::function<void()> toRun,
                               std::initializer_list<Subsystem*> requirements) {
+  return WhenInactive(std::move(toRun), wpi::makeArrayRef(requirements.begin(),
+                                                          requirements.end()));
+}
+
+Trigger Trigger::WhenInactive(std::function<void()> toRun,
+                              wpi::ArrayRef<Subsystem*> requirements) {
   return WhenInactive(InstantCommand(std::move(toRun), requirements));
 }
 

wpilibNewCommands/src/main/native/include/frc2/command/Command.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,6 +8,7 @@
 #pragma once
 
 #include <functional>
+#include <initializer_list>
 #include <memory>
 #include <string>
 
@@ -132,7 +133,18 @@ class Command : public frc::ErrorBase {
    */
   SequentialCommandGroup BeforeStarting(
       std::function<void()> toRun,
-      std::initializer_list<Subsystem*> requirements = {}) &&;
+      std::initializer_list<Subsystem*> requirements) &&;
+
+  /**
+   * Decorates this command with a runnable to run before this command starts.
+   *
+   * @param toRun the Runnable to run
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  SequentialCommandGroup BeforeStarting(
+      std::function<void()> toRun,
+      wpi::ArrayRef<Subsystem*> requirements = {}) &&;
 
   /**
    * Decorates this command with a runnable to run after the command finishes.
@@ -143,7 +155,18 @@ class Command : public frc::ErrorBase {
    */
   SequentialCommandGroup AndThen(
       std::function<void()> toRun,
-      std::initializer_list<Subsystem*> requirements = {}) &&;
+      std::initializer_list<Subsystem*> requirements) &&;
+
+  /**
+   * Decorates this command with a runnable to run after the command finishes.
+   *
+   * @param toRun the Runnable to run
+   * @param requirements the required subsystems
+   * @return the decorated command
+   */
+  SequentialCommandGroup AndThen(
+      std::function<void()> toRun,
+      wpi::ArrayRef<Subsystem*> requirements = {}) &&;
 
   /**
    * Decorates this command to run perpetually, ignoring its ordinary end

wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,6 +12,7 @@
 
 #include <frc/smartdashboard/Sendable.h>
 #include <frc/smartdashboard/SendableHelper.h>
+#include <wpi/ArrayRef.h>
 #include <wpi/SmallSet.h>
 #include <wpi/Twine.h>
 
@@ -32,6 +33,13 @@ class CommandBase : public Command,
    */
   void AddRequirements(std::initializer_list<Subsystem*> requirements);
 
+  /**
+   * Adds the specified requirements to the command.
+   *
+   * @param requirements the requirements to add
+   */
+  void AddRequirements(wpi::ArrayRef<Subsystem*> requirements);
+
   void AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements);
 
   wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;

wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -157,8 +157,10 @@ class CommandScheduler final : public frc::Sendable,
   void UnregisterSubsystem(Subsystem* subsystem);
 
   void RegisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+  void RegisterSubsystem(wpi::ArrayRef<Subsystem*> subsystems);
 
   void UnregisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+  void UnregisterSubsystem(wpi::ArrayRef<Subsystem*> subsystems);
 
   /**
    * Sets the default command for a subsystem.  Registers that subsystem if it

wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,6 +8,9 @@
 #pragma once
 
 #include <functional>
+#include <initializer_list>
+
+#include <wpi/ArrayRef.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -36,7 +39,23 @@ class FunctionalCommand : public CommandHelper<CommandBase, FunctionalCommand> {
                     std::function<void()> onExecute,
                     std::function<void(bool)> onEnd,
                     std::function<bool()> isFinished,
-                    std::initializer_list<Subsystem*> requirements = {});
+                    std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new FunctionalCommand.
+   *
+   * @param onInit       the function to run on command initialization
+   * @param onExecute    the function to run on command execution
+   * @param onEnd        the function to run on command end
+   * @param isFinished   the function that determines whether the command has
+   * finished
+   * @param requirements the subsystems required by this command
+   */
+  FunctionalCommand(std::function<void()> onInit,
+                    std::function<void()> onExecute,
+                    std::function<void(bool)> onEnd,
+                    std::function<bool()> isFinished,
+                    wpi::ArrayRef<Subsystem*> requirements = {});
 
   FunctionalCommand(FunctionalCommand&& other) = default;
 

wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,6 +10,8 @@
 #include <functional>
 #include <initializer_list>
 
+#include <wpi/ArrayRef.h>
+
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 
@@ -29,7 +31,17 @@ class InstantCommand : public CommandHelper<CommandBase, InstantCommand> {
    * @param requirements the subsystems required by this command
    */
   InstantCommand(std::function<void()> toRun,
-                 std::initializer_list<Subsystem*> requirements = {});
+                 std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new InstantCommand that runs the given Runnable with the given
+   * requirements.
+   *
+   * @param toRun        the Runnable to run
+   * @param requirements the subsystems required by this command
+   */
+  InstantCommand(std::function<void()> toRun,
+                 wpi::ArrayRef<Subsystem*> requirements = {});
 
   InstantCommand(InstantCommand&& other) = default;
 

wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,7 @@
 
 #include <cmath>
 #include <functional>
+#include <initializer_list>
 #include <memory>
 
 #include <frc/controller/PIDController.h>
@@ -18,6 +19,7 @@
 #include <frc/kinematics/MecanumDriveWheelSpeeds.h>
 #include <frc/trajectory/Trajectory.h>
 #include <units/units.h>
+#include <wpi/ArrayRef.h>
 
 #include "CommandBase.h"
 #include "CommandHelper.h"
@@ -99,6 +101,57 @@ class MecanumControllerCommand
           output,
       std::initializer_list<Subsystem*> requirements);
 
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow
+   * the provided trajectory. PID control and feedforward are handled
+   * internally. Outputs are scaled from -12 to 12 as a voltage output to the
+   * motor.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The rotation controller will calculate the rotation based on the
+   * final pose in the trajectory, not the poses at each time step.
+   *
+   * @param trajectory           The trajectory to follow.
+   * @param pose                 A function that supplies the robot pose,
+   *                             provided by the odometry class.
+   * @param feedforward          The feedforward to use for the drivetrain.
+   * @param kinematics           The kinematics for the robot drivetrain.
+   * @param xController          The Trajectory Tracker PID controller
+   *                             for the robot's x position.
+   * @param yController          The Trajectory Tracker PID controller
+   *                             for the robot's y position.
+   * @param thetaController      The Trajectory Tracker PID controller
+   *                             for angle for the robot.
+   * @param maxWheelVelocity     The maximum velocity of a drivetrain wheel.
+   * @param frontLeftController  The front left wheel velocity PID.
+   * @param rearLeftController   The rear left wheel velocity PID.
+   * @param frontRightController The front right wheel velocity PID.
+   * @param rearRightController  The rear right wheel velocity PID.
+   * @param currentWheelSpeeds   A MecanumDriveWheelSpeeds object containing
+   *                             the current wheel speeds.
+   * @param output               The output of the velocity PIDs.
+   * @param requirements         The subsystems to require.
+   */
+  MecanumControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SimpleMotorFeedforward<units::meters> feedforward,
+      frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+      frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      units::meters_per_second_t maxWheelVelocity,
+      std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+      frc2::PIDController frontLeftController,
+      frc2::PIDController rearLeftController,
+      frc2::PIDController frontRightController,
+      frc2::PIDController rearRightController,
+      std::function<void(units::volt_t, units::volt_t, units::volt_t,
+                         units::volt_t)>
+          output,
+      wpi::ArrayRef<Subsystem*> requirements = {});
+
   /**
    * Constructs a new MecanumControllerCommand that when executed will follow
    * the provided trajectory. The user should implement a velocity PID on the
@@ -137,6 +190,44 @@ class MecanumControllerCommand
           output,
       std::initializer_list<Subsystem*> requirements);
 
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow
+   * the provided trajectory. The user should implement a velocity PID on the
+   * desired output wheel velocities.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path - this is left to the user, since it is not
+   * appropriate for paths with non-stationary end-states.
+   *
+   * <p>Note2: The rotation controller will calculate the rotation based on the
+   * final pose in the trajectory, not the poses at each time step.
+   *
+   * @param trajectory       The trajectory to follow.
+   * @param pose             A function that supplies the robot pose - use one
+   * of the odometry classes to provide this.
+   * @param kinematics       The kinematics for the robot drivetrain.
+   * @param xController      The Trajectory Tracker PID controller
+   *                         for the robot's x position.
+   * @param yController      The Trajectory Tracker PID controller
+   *                         for the robot's y position.
+   * @param thetaController  The Trajectory Tracker PID controller
+   *                         for angle for the robot.
+   * @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
+   * @param output           The output of the position PIDs.
+   * @param requirements     The subsystems to require.
+   */
+  MecanumControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+      frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      units::meters_per_second_t maxWheelVelocity,
+      std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+                         units::meters_per_second_t,
+                         units::meters_per_second_t)>
+          output,
+      wpi::ArrayRef<Subsystem*> requirements = {});
+
   void Initialize() override;
 
   void Execute() override;

wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,6 +12,7 @@
 
 #include <frc/Notifier.h>
 #include <units/units.h>
+#include <wpi/ArrayRef.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -37,7 +38,17 @@ class NotifierCommand : public CommandHelper<CommandBase, NotifierCommand> {
    * @param requirements the subsystems required by this command
    */
   NotifierCommand(std::function<void()> toRun, units::second_t period,
-                  std::initializer_list<Subsystem*> requirements = {});
+                  std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new NotifierCommand.
+   *
+   * @param toRun        the runnable for the notifier to run
+   * @param period       the period at which the notifier should run
+   * @param requirements the subsystems required by this command
+   */
+  NotifierCommand(std::function<void()> toRun, units::second_t period,
+                  wpi::ArrayRef<Subsystem*> requirements = {});
 
   NotifierCommand(NotifierCommand&& other);
 

wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -40,7 +40,23 @@ class PIDCommand : public CommandHelper<CommandBase, PIDCommand> {
              std::function<double()> measurementSource,
              std::function<double()> setpointSource,
              std::function<void(double)> useOutput,
-             std::initializer_list<Subsystem*> requirements = {});
+             std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * PIDController.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param setpointSource   the controller's reference (aka setpoint)
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  PIDCommand(PIDController controller,
+             std::function<double()> measurementSource,
+             std::function<double()> setpointSource,
+             std::function<void(double)> useOutput,
+             wpi::ArrayRef<Subsystem*> requirements = {});
 
   /**
    * Creates a new PIDCommand, which controls the given output with a
@@ -57,6 +73,21 @@ class PIDCommand : public CommandHelper<CommandBase, PIDCommand> {
              std::function<void(double)> useOutput,
              std::initializer_list<Subsystem*> requirements);
 
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * PIDController with a constant setpoint.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param setpoint         the controller's setpoint (aka setpoint)
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  PIDCommand(PIDController controller,
+             std::function<double()> measurementSource, double setpoint,
+             std::function<void(double)> useOutput,
+             wpi::ArrayRef<Subsystem*> requirements = {});
+
   PIDCommand(PIDCommand&& other) = default;
 
   PIDCommand(const PIDCommand& other) = default;

wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h

@@ -24,8 +24,9 @@ class PIDSubsystem : public SubsystemBase {
    * Creates a new PIDSubsystem.
    *
    * @param controller the PIDController to use
+   * @param initialPosition the initial setpoint of the subsystem
    */
-  explicit PIDSubsystem(PIDController controller);
+  explicit PIDSubsystem(PIDController controller, double initialPosition = 0);
 
   void Periodic() override;
 
@@ -62,7 +63,7 @@ class PIDSubsystem : public SubsystemBase {
 
  protected:
   PIDController m_controller;
-  bool m_enabled;
+  bool m_enabled{false};
 
   /**
    * Returns the measurement of the process variable used by the PIDController.

wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,6 +13,7 @@
 
 #include <frc/controller/ProfiledPIDController.h>
 #include <units/units.h>
+#include <wpi/ArrayRef.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -50,7 +51,29 @@ class ProfiledPIDCommand
                      std::function<Distance_t()> measurementSource,
                      std::function<State()> goalSource,
                      std::function<void(double, State)> useOutput,
-                     std::initializer_list<Subsystem*> requirements = {})
+                     std::initializer_list<Subsystem*> requirements)
+      : m_controller{controller},
+        m_measurement{std::move(measurementSource)},
+        m_goal{std::move(goalSource)},
+        m_useOutput{std::move(useOutput)} {
+    this->AddRequirements(requirements);
+  }
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goalSource   the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+                     std::function<Distance_t()> measurementSource,
+                     std::function<State()> goalSource,
+                     std::function<void(double, State)> useOutput,
+                     wpi::ArrayRef<Subsystem*> requirements = {})
       : m_controller{controller},
         m_measurement{std::move(measurementSource)},
         m_goal{std::move(goalSource)},
@@ -79,6 +102,27 @@ class ProfiledPIDCommand
                            },
                            useOutput, requirements) {}
 
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goalSource   the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+                     std::function<Distance_t()> measurementSource,
+                     std::function<Distance_t()> goalSource,
+                     std::function<void(double, State)> useOutput,
+                     wpi::ArrayRef<Subsystem*> requirements = {})
+      : ProfiledPIDCommand(controller, measurementSource,
+                           [&goalSource]() {
+                             return State{goalSource(), Velocity_t{0}};
+                           },
+                           useOutput, requirements) {}
+
   /**
    * Creates a new PIDCommand, which controls the given output with a
    * ProfiledPIDController with a constant goal.
@@ -96,6 +140,23 @@ class ProfiledPIDCommand
       : ProfiledPIDCommand(controller, measurementSource,
                            [goal] { return goal; }, useOutput, requirements) {}
 
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController with a constant goal.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goal         the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+                     std::function<Distance_t()> measurementSource, State goal,
+                     std::function<void(double, State)> useOutput,
+                     wpi::ArrayRef<Subsystem*> requirements = {})
+      : ProfiledPIDCommand(controller, measurementSource,
+                           [goal] { return goal; }, useOutput, requirements) {}
+
   /**
    * Creates a new PIDCommand, which controls the given output with a
    * ProfiledPIDController with a constant goal.
@@ -114,11 +175,29 @@ class ProfiledPIDCommand
       : ProfiledPIDCommand(controller, measurementSource,
                            [goal] { return goal; }, useOutput, requirements) {}
 
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController with a constant goal.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goal         the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+                     std::function<Distance_t()> measurementSource,
+                     Distance_t goal,
+                     std::function<void(double, State)> useOutput,
+                     wpi::ArrayRef<Subsystem*> requirements = {})
+      : ProfiledPIDCommand(controller, measurementSource,
+                           [goal] { return goal; }, useOutput, requirements) {}
+
   ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
 
   ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
 
-  void Initialize() override { m_controller.Reset(); }
+  void Initialize() override { m_controller.Reset(m_measurement()); }
 
   void Execute() override {
     m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),

wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -32,9 +32,13 @@ class ProfiledPIDSubsystem : public SubsystemBase {
    * Creates a new ProfiledPIDSubsystem.
    *
    * @param controller the ProfiledPIDController to use
+   * @param initialPosition the initial goal position of the subsystem
    */
-  explicit ProfiledPIDSubsystem(frc::ProfiledPIDController<Distance> controller)
-      : m_controller{controller} {}
+  explicit ProfiledPIDSubsystem(frc::ProfiledPIDController<Distance> controller,
+                                Distance_t initialPosition = Distance_t{0})
+      : m_controller{controller} {
+    SetGoal(initialPosition);
+  }
 
   void Periodic() override {
     if (m_enabled) {
@@ -58,10 +62,10 @@ class ProfiledPIDSubsystem : public SubsystemBase {
   void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
 
   /**
-   * Enables the PID control.  Resets the controller.
+   * Enables the PID control. Resets the controller.
    */
   virtual void Enable() {
-    m_controller.Reset();
+    m_controller.Reset(GetMeasurement());
     m_enabled = true;
   }
 

wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,6 +18,7 @@
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/trajectory/Trajectory.h>
 #include <units/units.h>
+#include <wpi/ArrayRef.h>
 
 #include "frc2/Timer.h"
 #include "frc2/command/CommandBase.h"
@@ -78,7 +79,44 @@ class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
                  frc2::PIDController leftController,
                  frc2::PIDController rightController,
                  std::function<void(units::volt_t, units::volt_t)> output,
-                 std::initializer_list<Subsystem*> requirements = {});
+                 std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the
+   * provided trajectory. PID control and feedforward are handled internally,
+   * and outputs are scaled -12 to 12 representing units of volts.
+   *
+   * <p>Note: The controller will *not* set the outputVolts to zero upon
+   * completion of the path - this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose - use one of
+   * the odometry classes to provide this.
+   * @param controller      The RAMSETE controller used to follow the
+   * trajectory.
+   * @param feedforward     A component for calculating the feedforward for the
+   * drive.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param wheelSpeeds     A function that supplies the speeds of the left
+   * and right sides of the robot drive.
+   * @param leftController  The PIDController for the left side of the robot
+   * drive.
+   * @param rightController The PIDController for the right side of the robot
+   * drive.
+   * @param output          A function that consumes the computed left and right
+   * outputs (in volts) for the robot drive.
+   * @param requirements    The subsystems to require.
+   */
+  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+                 frc::RamseteController controller,
+                 frc::SimpleMotorFeedforward<units::meters> feedforward,
+                 frc::DifferentialDriveKinematics kinematics,
+                 std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
+                 frc2::PIDController leftController,
+                 frc2::PIDController rightController,
+                 std::function<void(units::volt_t, units::volt_t)> output,
+                 wpi::ArrayRef<Subsystem*> requirements = {});
 
   /**
    * Constructs a new RamseteCommand that, when executed, will follow the
@@ -104,6 +142,30 @@ class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
                      output,
                  std::initializer_list<Subsystem*> requirements);
 
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the
+   * provided trajectory. Performs no PID control and calculates no
+   * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller,
+   * and will need to be converted into a usable form by the user.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose - use one of
+   * the odometry classes to provide this.
+   * @param controller      The RAMSETE controller used to follow the
+   * trajectory.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param output          A function that consumes the computed left and right
+   * wheel speeds.
+   * @param requirements    The subsystems to require.
+   */
+  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+                 frc::RamseteController controller,
+                 frc::DifferentialDriveKinematics kinematics,
+                 std::function<void(units::meters_per_second_t,
+                                    units::meters_per_second_t)>
+                     output,
+                 wpi::ArrayRef<Subsystem*> requirements = {});
+
   void Initialize() override;
 
   void Execute() override;

wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,6 +10,8 @@
 #include <functional>
 #include <initializer_list>
 
+#include <wpi/ArrayRef.h>
+
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 
@@ -30,7 +32,17 @@ class RunCommand : public CommandHelper<CommandBase, RunCommand> {
    * @param requirements the subsystems to require
    */
   RunCommand(std::function<void()> toRun,
-             std::initializer_list<Subsystem*> requirements = {});
+             std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new RunCommand.  The Runnable will be run continuously until the
+   * command ends.  Does not run when disabled.
+   *
+   * @param toRun        the Runnable to run
+   * @param requirements the subsystems to require
+   */
+  RunCommand(std::function<void()> toRun,
+             wpi::ArrayRef<Subsystem*> requirements = {});
 
   RunCommand(RunCommand&& other) = default;
 

wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,6 +10,8 @@
 #include <functional>
 #include <initializer_list>
 
+#include <wpi/ArrayRef.h>
+
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 
@@ -32,7 +34,18 @@ class StartEndCommand : public CommandHelper<CommandBase, StartEndCommand> {
    * @param requirements the subsystems required by this command
    */
   StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
-                  std::initializer_list<Subsystem*> requirements = {});
+                  std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new StartEndCommand.  Will run the given runnables when the
+   * command starts and when it ends.
+   *
+   * @param onInit       the Runnable to run on command init
+   * @param onEnd        the Runnable to run on command end
+   * @param requirements the subsystems required by this command
+   */
+  StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
+                  wpi::ArrayRef<Subsystem*> requirements = {});
 
   StartEndCommand(StartEndCommand&& other) = default;
 

wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,7 @@
 
 #include <cmath>
 #include <functional>
+#include <initializer_list>
 #include <memory>
 
 #include <frc/controller/PIDController.h>
@@ -17,6 +18,7 @@
 #include <frc/kinematics/SwerveModuleState.h>
 #include <frc/trajectory/Trajectory.h>
 #include <units/units.h>
+#include <wpi/ArrayRef.h>
 
 #include "CommandBase.h"
 #include "CommandHelper.h"
@@ -92,6 +94,42 @@ class SwerveControllerCommand
           output,
       std::initializer_list<Subsystem*> requirements);
 
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the
+   * provided trajectory. This command will not return output voltages but
+   * rather raw module states from the position controllers which need to be put
+   * into a velocity PID.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path- this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The rotation controller will calculate the rotation based on the
+   * final pose in the trajectory, not the poses at each time step.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose,
+   *                        provided by the odometry class.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param xController     The Trajectory Tracker PID controller
+   *                        for the robot's x position.
+   * @param yController     The Trajectory Tracker PID controller
+   *                        for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller
+   *                        for angle for the robot.
+   * @param output          The raw output module states from the
+   *                        position controllers.
+   * @param requirements    The subsystems to require.
+   */
+  SwerveControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SwerveDriveKinematics<NumModules> kinematics,
+      frc2::PIDController xController, frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+          output,
+      wpi::ArrayRef<Subsystem*> requirements = {});
+
   void Initialize() override;
 
   void Execute() override;

wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -31,6 +31,26 @@ SwerveControllerCommand<NumModules>::SwerveControllerCommand(
   this->AddRequirements(requirements);
 }
 
+template <size_t NumModules>
+SwerveControllerCommand<NumModules>::SwerveControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SwerveDriveKinematics<NumModules> kinematics,
+    frc2::PIDController xController, frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
+    wpi::ArrayRef<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_kinematics(kinematics),
+      m_xController(std::make_unique<frc2::PIDController>(xController)),
+      m_yController(std::make_unique<frc2::PIDController>(yController)),
+      m_thetaController(
+          std::make_unique<frc::ProfiledPIDController<units::radians>>(
+              thetaController)),
+      m_outputStates(output) {
+  this->AddRequirements(requirements);
+}
+
 template <size_t NumModules>
 void SwerveControllerCommand<NumModules>::Initialize() {
   m_finalPose = m_trajectory.Sample(m_trajectory.TotalTime()).pose;

wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,6 +11,7 @@
 #include <initializer_list>
 
 #include <frc/trajectory/TrapezoidProfile.h>
+#include <wpi/ArrayRef.h>
 
 #include "frc2/Timer.h"
 #include "frc2/command/CommandBase.h"
@@ -47,6 +48,20 @@ class TrapezoidProfileCommand
     this->AddRequirements(requirements);
   }
 
+  /**
+   * Creates a new TrapezoidProfileCommand that will execute the given
+   * TrapezoidalProfile. Output will be piped to the provided consumer function.
+   *
+   * @param profile The motion profile to execute.
+   * @param output  The consumer for the profile output.
+   */
+  TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
+                          std::function<void(State)> output,
+                          wpi::ArrayRef<Subsystem*> requirements = {})
+      : m_profile(profile), m_output(output) {
+    this->AddRequirements(requirements);
+  }
+
   void Initialize() override {
     m_timer.Reset();
     m_timer.Start();

wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h

@@ -37,18 +37,21 @@ class TrapezoidProfileSubsystem : public SubsystemBase {
    * when the subsystem is constructed.
    * @param period          The period of the main robot loop, in seconds.
    */
-  TrapezoidProfileSubsystem(Constraints constraints, Distance_t position,
-                            units::second_t period = 20_ms)
+  explicit TrapezoidProfileSubsystem(Constraints constraints,
+                                     Distance_t initialPosition = Distance_t{0},
+                                     units::second_t period = 20_ms)
       : m_constraints(constraints),
-        m_state{position, Velocity_t(0)},
-        m_goal{position, Velocity_t{0}},
+        m_state{initialPosition, Velocity_t(0)},
+        m_goal{initialPosition, Velocity_t{0}},
         m_period(period) {}
 
   void Periodic() override {
     auto profile =
         frc::TrapezoidProfile<Distance>(m_constraints, m_goal, m_state);
     m_state = profile.Calculate(m_period);
-    UseState(m_state);
+    if (m_enabled) {
+      UseState(m_state);
+    }
   }
 
   /**
@@ -74,10 +77,21 @@ class TrapezoidProfileSubsystem : public SubsystemBase {
    */
   virtual void UseState(State state) = 0;
 
+  /**
+   * Enable the TrapezoidProfileSubsystem's output.
+   */
+  void Enable() { m_enabled = true; }
+
+  /**
+   * Disable the TrapezoidProfileSubsystem's output.
+   */
+  void Disable() { m_enabled = false; }
+
  private:
   Constraints m_constraints;
   State m_state;
   State m_goal;
   units::second_t m_period;
+  bool m_enabled{false};
 };
 }  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h

@@ -1,13 +1,18 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
 #pragma once
+
+#include <functional>
+#include <initializer_list>
 #include <utility>
 
+#include <wpi/ArrayRef.h>
+
 #include "Trigger.h"
 
 namespace frc2 {
@@ -68,7 +73,16 @@ class Button : public Trigger {
    * @param requirements the required subsystems.
    */
   Button WhenPressed(std::function<void()> toRun,
-                     std::initializer_list<Subsystem*> requirements = {});
+                     std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Binds a runnable to execute when the button is pressed.
+   *
+   * @param toRun the runnable to execute.
+   * @param requirements the required subsystems.
+   */
+  Button WhenPressed(std::function<void()> toRun,
+                     wpi::ArrayRef<Subsystem*> requirements = {});
 
   /**
    * Binds a command to be started repeatedly while the button is pressed, and
@@ -105,7 +119,16 @@ class Button : public Trigger {
    * @param requirements the required subsystems.
    */
   Button WhileHeld(std::function<void()> toRun,
-                   std::initializer_list<Subsystem*> requirements = {});
+                   std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Binds a runnable to execute repeatedly while the button is pressed.
+   *
+   * @param toRun the runnable to execute.
+   * @param requirements the required subsystems.
+   */
+  Button WhileHeld(std::function<void()> toRun,
+                   wpi::ArrayRef<Subsystem*> requirements = {});
 
   /**
    * Binds a command to be started when the button is pressed, and cancelled
@@ -170,7 +193,16 @@ class Button : public Trigger {
    * @param requirements the required subsystems.
    */
   Button WhenReleased(std::function<void()> toRun,
-                      std::initializer_list<Subsystem*> requirements = {});
+                      std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Binds a runnable to execute when the button is released.
+   *
+   * @param toRun the runnable to execute.
+   * @param requirements the required subsystems.
+   */
+  Button WhenReleased(std::function<void()> toRun,
+                      wpi::ArrayRef<Subsystem*> requirements = {});
 
   /**
    * Binds a command to start when the button is pressed, and be cancelled when

wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,10 +7,13 @@
 
 #pragma once
 
-#include <atomic>
+#include <functional>
+#include <initializer_list>
 #include <memory>
 #include <utility>
 
+#include <wpi/ArrayRef.h>
+
 #include "frc2/command/Command.h"
 #include "frc2/command/CommandScheduler.h"
 
@@ -99,7 +102,16 @@ class Trigger {
    * @paaram requirements the required subsystems.
    */
   Trigger WhenActive(std::function<void()> toRun,
-                     std::initializer_list<Subsystem*> requirements = {});
+                     std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Binds a runnable to execute when the trigger becomes active.
+   *
+   * @param toRun the runnable to execute.
+   * @paaram requirements the required subsystems.
+   */
+  Trigger WhenActive(std::function<void()> toRun,
+                     wpi::ArrayRef<Subsystem*> requirements = {});
 
   /**
    * Binds a command to be started repeatedly while the trigger is active, and
@@ -149,9 +161,17 @@ class Trigger {
    * @param toRun the runnable to execute.
    * @param requirements the required subsystems.
    */
-  Trigger WhileActiveContinous(
-      std::function<void()> toRun,
-      std::initializer_list<Subsystem*> requirements = {});
+  Trigger WhileActiveContinous(std::function<void()> toRun,
+                               std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Binds a runnable to execute repeatedly while the trigger is active.
+   *
+   * @param toRun the runnable to execute.
+   * @param requirements the required subsystems.
+   */
+  Trigger WhileActiveContinous(std::function<void()> toRun,
+                               wpi::ArrayRef<Subsystem*> requirements = {});
 
   /**
    * Binds a command to be started when the trigger becomes active, and
@@ -242,7 +262,16 @@ class Trigger {
    * @param requirements the required subsystems.
    */
   Trigger WhenInactive(std::function<void()> toRun,
-                       std::initializer_list<Subsystem*> requirements = {});
+                       std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Binds a runnable to execute when the trigger becomes inactive.
+   *
+   * @param toRun the runnable to execute.
+   * @param requirements the required subsystems.
+   */
+  Trigger WhenInactive(std::function<void()> toRun,
+                       wpi::ArrayRef<Subsystem*> requirements = {});
 
   /**
    * Binds a command to start when the trigger becomes active, and be cancelled

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java

@@ -184,7 +184,6 @@ public abstract class Subsystem implements Sendable, AutoCloseable {
    */
   public void addChild(String name, Sendable child) {
     SendableRegistry.addLW(child, getSubsystem(), name);
-    SendableRegistry.addChild(this, child);
   }
 
   /**
@@ -195,7 +194,6 @@ public abstract class Subsystem implements Sendable, AutoCloseable {
   public void addChild(Sendable child) {
     SendableRegistry.setSubsystem(child, getSubsystem());
     SendableRegistry.enableLiveWindow(child);
-    SendableRegistry.addChild(this, child);
   }
 
   /**

wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp

@@ -101,7 +101,6 @@ void Subsystem::AddChild(const wpi::Twine& name, Sendable* child) {
 void Subsystem::AddChild(const wpi::Twine& name, Sendable& child) {
   auto& registry = SendableRegistry::GetInstance();
   registry.AddLW(&child, registry.GetSubsystem(this), name);
-  registry.AddChild(this, &child);
 }
 
 void Subsystem::AddChild(std::shared_ptr<Sendable> child) { AddChild(*child); }
@@ -112,7 +111,6 @@ void Subsystem::AddChild(Sendable& child) {
   auto& registry = SendableRegistry::GetInstance();
   registry.SetSubsystem(&child, registry.GetSubsystem(this));
   registry.EnableLiveWindow(&child);
-  registry.AddChild(this, &child);
 }
 
 void Subsystem::ConfirmCommand() {

wpilibc/build.gradle

@@ -231,6 +231,8 @@ model {
     }
 }
 
+apply from: "${rootDir}/shared/cppDesktopTestTask.gradle"
+
 tasks.withType(RunTestExecutable) {
     args "--gtest_output=xml:test_detail.xml"
     outputs.dir outputDir

wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp

@@ -20,7 +20,9 @@ DutyCycleEncoder::DutyCycleEncoder(int channel)
     : m_dutyCycle{std::make_shared<DutyCycle>(
           std::make_shared<DigitalInput>(channel))},
       m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {}
+      m_counter{} {
+  Init();
+}
 
 DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle)
     : m_dutyCycle{&dutyCycle, NullDeleter<DutyCycle>{}},

wpilibc/src/main/native/cpp/PWMTalonFX.cpp

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWMTalonFX.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PWMTalonFX::PWMTalonFX(int channel) : PWMSpeedController(channel) {
+  SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+  SetPeriodMultiplier(kPeriodMultiplier_1X);
+  SetSpeed(0.0);
+  SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_TalonFX, GetChannel() + 1);
+  SendableRegistry::GetInstance().SetName(this, "PWMTalonFX", GetChannel());
+}

wpilibc/src/main/native/cpp/PWMVenom.cpp

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWMVenom.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PWMVenom::PWMVenom(int channel) : PWMSpeedController(channel) {
+  SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+  SetPeriodMultiplier(kPeriodMultiplier_1X);
+  SetSpeed(0.0);
+  SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_FusionVenom, GetChannel() + 1);
+  SendableRegistry::GetInstance().SetName(this, "PWMVenom", GetChannel());
+}

wpilibc/src/main/native/cpp/SPI.cpp

@@ -311,6 +311,14 @@ int SPI::GetAutoDroppedCount() {
   return val;
 }
 
+void SPI::ConfigureAutoStall(HAL_SPIPort port, int csToSclkTicks,
+                             int stallTicks, int pow2BytesPerRead) {
+  int32_t status = 0;
+  HAL_ConfigureSPIAutoStall(m_port, csToSclkTicks, stallTicks, pow2BytesPerRead,
+                            &status);
+  wpi_setHALError(status);
+}
+
 void SPI::InitAccumulator(units::second_t period, int cmd, int xferSize,
                           int validMask, int validValue, int dataShift,
                           int dataSize, bool isSigned, bool bigEndian) {

wpilibc/src/main/native/cpp/controller/PIDController.cpp

@@ -22,7 +22,7 @@ PIDController::PIDController(double Kp, double Ki, double Kd,
     : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
   static int instances = 0;
   instances++;
-  HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
+  HAL_Report(HALUsageReporting::kResourceType_PIDController2, instances);
   frc::SendableRegistry::GetInstance().Add(this, "PIDController", instances);
 }
 

wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp

@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/ProfiledPIDController.h"
+
+#include <hal/FRCUsageReporting.h>
+
+void frc::detail::ReportProfiledPIDController() {
+  static int instances = 0;
+  ++instances;
+  HAL_Report(HALUsageReporting::kResourceType_ProfiledPIDController, instances);
+}

wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp

@@ -7,6 +7,8 @@
 
 #include "frc/kinematics/DifferentialDriveOdometry.h"
 
+#include <hal/FRCUsageReporting.h>
+
 using namespace frc;
 
 DifferentialDriveOdometry::DifferentialDriveOdometry(
@@ -14,6 +16,8 @@ DifferentialDriveOdometry::DifferentialDriveOdometry(
     : m_pose(initialPose) {
   m_previousAngle = m_pose.Rotation();
   m_gyroOffset = m_pose.Rotation() - gyroAngle;
+  HAL_Report(HALUsageReporting::kResourceType_Odometry,
+             HALUsageReporting::kOdometry_DifferentialDrive);
 }
 
 const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,

wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp

@@ -7,6 +7,8 @@
 
 #include "frc/kinematics/MecanumDriveOdometry.h"
 
+#include <hal/FRCUsageReporting.h>
+
 using namespace frc;
 
 MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
@@ -15,6 +17,8 @@ MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
     : m_kinematics(kinematics), m_pose(initialPose) {
   m_previousAngle = m_pose.Rotation();
   m_gyroOffset = m_pose.Rotation() - gyroAngle;
+  HAL_Report(HALUsageReporting::kResourceType_Odometry,
+             HALUsageReporting::kOdometry_MecanumDrive);
 }
 
 const Pose2d& MecanumDriveOdometry::UpdateWithTime(

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

@@ -134,6 +134,12 @@ void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& subsystem,
   comp.subsystem = subsystem.str();
 }
 
+void SendableRegistry::AddChild(Sendable* parent, Sendable* child) {
+  std::scoped_lock lock(m_impl->mutex);
+  auto& comp = m_impl->GetOrAdd(child);
+  comp.parent = parent;
+}
+
 void SendableRegistry::AddChild(Sendable* parent, void* child) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(child);
@@ -147,6 +153,10 @@ bool SendableRegistry::Remove(Sendable* sendable) {
   UID compUid = it->getSecond();
   m_impl->components.erase(compUid - 1);
   m_impl->componentMap.erase(it);
+  // update any parent pointers
+  for (auto&& comp : m_impl->components) {
+    if (comp->parent == sendable) comp->parent = nullptr;
+  }
   return true;
 }
 
@@ -164,6 +174,10 @@ void SendableRegistry::Move(Sendable* to, Sendable* from) {
     comp.builder.ClearProperties();
     to->InitSendable(comp.builder);
   }
+  // update any parent pointers
+  for (auto&& comp : m_impl->components) {
+    if (comp->parent == from) comp->parent = to;
+  }
 }
 
 bool SendableRegistry::Contains(const Sendable* sendable) const {

wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp

@@ -107,6 +107,33 @@ Trajectory::State Trajectory::Sample(units::second_t t) const {
                                 (t - prevSample.t) / (sample.t - prevSample.t));
 }
 
+Trajectory Trajectory::TransformBy(const Transform2d& transform) {
+  auto& firstState = m_states[0];
+  auto& firstPose = firstState.pose;
+
+  // Calculate the transformed first pose.
+  auto newFirstPose = firstPose + transform;
+  auto newStates = m_states;
+  newStates[0].pose = newFirstPose;
+
+  for (unsigned int i = 1; i < newStates.size(); i++) {
+    auto& state = newStates[i];
+    // We are transforming relative to the coordinate frame of the new initial
+    // pose.
+    state.pose = newFirstPose + (state.pose - firstPose);
+  }
+
+  return Trajectory(newStates);
+}
+
+Trajectory Trajectory::RelativeTo(const Pose2d& pose) {
+  auto newStates = m_states;
+  for (auto& state : newStates) {
+    state.pose = state.pose.RelativeTo(pose);
+  }
+  return Trajectory(newStates);
+}
+
 void frc::to_json(wpi::json& json, const Trajectory::State& state) {
   json = wpi::json{{"time", state.t.to<double>()},
                    {"velocity", state.velocity.to<double>()},

wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,11 +9,16 @@
 
 #include <utility>
 
+#include "frc/DriverStation.h"
 #include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
 #include "frc/trajectory/TrajectoryParameterizer.h"
 
 using namespace frc;
 
+const Trajectory TrajectoryGenerator::kDoNothingTrajectory(
+    std::vector<Trajectory::State>{Trajectory::State()});
+
 Trajectory TrajectoryGenerator::GenerateTrajectory(
     Spline<3>::ControlVector initial,
     const std::vector<Translation2d>& interiorWaypoints,
@@ -29,9 +34,15 @@ Trajectory TrajectoryGenerator::GenerateTrajectory(
     end.y[1] *= -1;
   }
 
-  auto points =
-      SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
-          initial, interiorWaypoints, end));
+  std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
+  try {
+    points =
+        SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
+            initial, interiorWaypoints, end));
+  } catch (SplineParameterizer::MalformedSplineException& e) {
+    DriverStation::ReportError(e.what());
+    return kDoNothingTrajectory;
+  }
 
   // After trajectory generation, flip theta back so it's relative to the
   // field. Also fix curvature.
@@ -68,8 +79,14 @@ Trajectory TrajectoryGenerator::GenerateTrajectory(
     }
   }
 
-  auto points = SplinePointsFromSplines(
-      SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
+  std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
+  try {
+    points = SplinePointsFromSplines(
+        SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
+  } catch (SplineParameterizer::MalformedSplineException& e) {
+    DriverStation::ReportError(e.what());
+    return kDoNothingTrajectory;
+  }
 
   // After trajectory generation, flip theta back so it's relative to the
   // field. Also fix curvature.

wpilibc/src/main/native/cppcs/RobotBase.cpp

@@ -36,7 +36,7 @@ int frc::RunHALInitialization() {
     return -1;
   }
   HAL_Report(HALUsageReporting::kResourceType_Language,
-             HALUsageReporting::kLanguage_CPlusPlus);
+             HALUsageReporting::kLanguage_CPlusPlus, 0, GetWPILibVersion());
   wpi::outs() << "\n********** Robot program starting **********\n";
   return 0;
 }
@@ -124,7 +124,11 @@ RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
 
   auto inst = nt::NetworkTableInstance::GetDefault();
   inst.SetNetworkIdentity("Robot");
+#ifdef __FRC_ROBORIO__
   inst.StartServer("/home/lvuser/networktables.ini");
+#else
+  inst.StartServer();
+#endif
 
   SmartDashboard::init();
 

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

@@ -15,6 +15,8 @@
 #include <wpi/ArrayRef.h>
 
 #include "frc/ErrorBase.h"
+#include "util/Color.h"
+#include "util/Color8Bit.h"
 
 namespace frc {
 
@@ -54,6 +56,28 @@ class AddressableLED : public ErrorBase {
      * @param v the v value [0-255]
      */
     void SetHSV(int h, int s, int v);
+
+    /*
+     * Sets a specific LED in the buffer.
+     *
+     * @param color The color of the LED
+     */
+    void SetLED(const Color& color) {
+      this->r = color.red * 255;
+      this->g = color.green * 255;
+      this->b = color.blue * 255;
+    }
+
+    /*
+     * Sets a specific LED in the buffer.
+     *
+     * @param color The color of the LED
+     */
+    void SetLED(const Color8Bit& color) {
+      this->r = color.red;
+      this->g = color.green;
+      this->b = color.blue;
+    }
   };
 
   /**

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

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/PWMSpeedController.h"
+
+namespace frc {
+
+/**
+ * Cross the Road Electronics (CTRE) Talon FX Speed Controller with PWM
+ * control.
+ *
+ * Note that the Talon FX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Talon FX User
+ * Manual available from Cross The Road Electronics.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
+ */
+class PWMTalonFX : public PWMSpeedController {
+ public:
+  /**
+   * Construct a Talon FX connected via PWM.
+   *
+   * @param channel The PWM channel that the Talon FX is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  explicit PWMTalonFX(int channel);
+
+  PWMTalonFX(PWMTalonFX&&) = default;
+  PWMTalonFX& operator=(PWMTalonFX&&) = default;
+};
+
+}  // namespace frc

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

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/PWMSpeedController.h"
+
+namespace frc {
+
+/**
+ * Playing with Fusion Venom Smart Motor with PWM control.
+ *
+ * Note that the Venom uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
+ */
+class PWMVenom : public PWMSpeedController {
+ public:
+  /**
+   * Construct a Venom connected via PWM.
+   *
+   * @param channel The PWM channel that the Venom is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  explicit PWMVenom(int channel);
+
+  PWMVenom(PWMVenom&&) = default;
+  PWMVenom& operator=(PWMVenom&&) = default;
+};
+
+}  // namespace frc

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

@@ -269,6 +269,19 @@ class SPI : public ErrorBase {
    */
   int GetAutoDroppedCount();
 
+  /**
+   * Configure the Auto SPI Stall time between reads.
+   *
+   * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+   * MXP.
+   * @param csToSclkTicks the number of ticks to wait before asserting the cs
+   * pin
+   * @param stallTicks the number of ticks to stall for
+   * @param pow2BytesPerRead the number of bytes to read before stalling
+   */
+  void ConfigureAutoStall(HAL_SPIPort port, int csToSclkTicks, int stallTicks,
+                          int pow2BytesPerRead);
+
   /**
    * Initialize the accumulator.
    *

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

@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/Timer.h>
+
+#include <algorithm>
+
+#include <units/units.h>
+
+namespace frc {
+/**
+ * A class that limits the rate of change of an input value.  Useful for
+ * implementing voltage, setpoint, and/or output ramps.  A slew-rate limit
+ * is most appropriate when the quantity being controlled is a velocity or
+ * a voltage; when controlling a position, consider using a TrapezoidProfile
+ * instead.
+ *
+ * @see TrapezoidProfile
+ */
+template <class Unit>
+class SlewRateLimiter {
+  using Unit_t = units::unit_t<Unit>;
+  using Rate = units::compound_unit<Unit, units::inverse<units::seconds>>;
+  using Rate_t = units::unit_t<Rate>;
+
+ public:
+  /**
+   * Creates a new SlewRateLimiter with the given rate limit and initial value.
+   *
+   * @param rateLimit The rate-of-change limit.
+   * @param initialValue The initial value of the input.
+   */
+  explicit SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue = Unit_t{0})
+      : m_rateLimit{rateLimit}, m_prevVal{initialValue} {
+    m_timer.Start();
+  }
+
+  /**
+   * Filters the input to limit its slew rate.
+   *
+   * @param input The input value whose slew rate is to be limited.
+   * @return The filtered value, which will not change faster than the slew
+   * rate.
+   */
+  Unit_t Calculate(Unit_t input) {
+    m_prevVal += std::clamp(input - m_prevVal, -m_rateLimit * m_timer.Get(),
+                            m_rateLimit * m_timer.Get());
+    m_timer.Reset();
+    return m_prevVal;
+  }
+
+  /**
+   * Resets the slew rate limiter to the specified value; ignores the rate limit
+   * when doing so.
+   *
+   * @param value The value to reset to.
+   */
+  void Reset(Unit_t value) {
+    m_timer.Reset();
+    m_prevVal = value;
+  }
+
+ private:
+  frc2::Timer m_timer;
+  Rate_t m_rateLimit;
+  Unit_t m_prevVal;
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,6 +21,9 @@
 #include "frc/trajectory/TrapezoidProfile.h"
 
 namespace frc {
+namespace detail {
+void ReportProfiledPIDController();
+}  // namespace detail
 
 /**
  * Implements a PID control loop whose setpoint is constrained by a trapezoid
@@ -43,7 +46,8 @@ class ProfiledPIDController
  public:
   /**
    * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
-   * Kd.
+   * Kd. Users should call reset() when they first start running the controller
+   * to avoid unwanted behavior.
    *
    * @param Kp          The proportional coefficient.
    * @param Ki          The integral coefficient.
@@ -54,7 +58,9 @@ class ProfiledPIDController
    */
   ProfiledPIDController(double Kp, double Ki, double Kd,
                         Constraints constraints, units::second_t period = 20_ms)
-      : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
+      : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
+    detail::ReportProfiledPIDController();
+  }
 
   ~ProfiledPIDController() override = default;
 
@@ -287,9 +293,34 @@ class ProfiledPIDController
   }
 
   /**
-   * Reset the previous error, the integral term, and disable the controller.
+   * Reset the previous error and the integral term.
+   *
+   * @param measurement The current measured State of the system.
    */
-  void Reset() { m_controller.Reset(); }
+  void Reset(const State& measurement) {
+    m_controller.Reset();
+    m_setpoint = measurement;
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system.
+   * @param measuredVelocity The current measured velocity of the system.
+   */
+  void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) {
+    Reset(State{measuredPosition, measuredVelocity});
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system. The
+   * velocity is assumed to be zero.
+   */
+  void Reset(Distance_t measuredPosition) {
+    Reset(measuredPosition, Velocity_t(0));
+  }
 
   void InitSendable(frc::SendableBuilder& builder) override {
     builder.SetSmartDashboardType("ProfiledPIDController");

wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <hal/FRCUsageReporting.h>
 #include <units/units.h>
 
 #include "frc/kinematics/ChassisSpeeds.h"
@@ -31,8 +32,11 @@ class DifferentialDriveKinematics {
    * empirical value may be larger than the physical measured value due to
    * scrubbing effects.
    */
-  constexpr explicit DifferentialDriveKinematics(units::meter_t trackWidth)
-      : trackWidth(trackWidth) {}
+  explicit DifferentialDriveKinematics(units::meter_t trackWidth)
+      : trackWidth(trackWidth) {
+    HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+               HALUsageReporting::kKinematics_DifferentialDrive);
+  }
 
   /**
    * Returns a chassis speed from left and right component velocities using