Fix LiveWindow SetEnabled C++ std::bad_function_call (#2281)

It was missing a null check.

.github/workflows/gradle-wrapper-validation.yml

@@ -0,0 +1,10 @@
+name: "Validate Gradle Wrapper"
+on: [push]
+
+jobs:
+  validation:
+    name: "Validation"
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+      - uses: gradle/wrapper-validation-action@v1

.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.

MavenArtifacts.md

@@ -5,8 +5,8 @@ WPILib publishes its built artifacts to our Maven server for use by downstream p
 ## Repositories
 We provide two repositories. These repositories are:
 
-* (Release)     https://first.wpi.edu/FRC/roborio/maven/release/
-* (Development) https://first.wpi.edu/FRC/roborio/maven/development/
+* (Release)     https://frcmaven.wpi.edu/artifactory/release/
+* (Development) https://frcmaven.wpi.edu/artifactory/development/
 
 The release repository is where official WPILib releases are pushed.
 The development repository is where development releases of every commit to [master](https://github.com/wpilibsuite/allwpilib/tree/master) is pushed.

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) {

cscore/src/main/native/linux/UsbCameraImpl.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-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.                                                               */
@@ -102,7 +102,7 @@ static bool IsPercentageProperty(wpi::StringRef name) {
   if (name.startswith("raw_")) name = name.substr(4);
   return name == "brightness" || name == "contrast" || name == "saturation" ||
          name == "hue" || name == "sharpness" || name == "gain" ||
-         name == "exposure_absolute";
+         name == "exposure_absolute" || name == "exposure_time_absolute";
 }
 
 static constexpr const int quirkLifeCamHd3000[] = {
@@ -112,6 +112,11 @@ static constexpr char const* quirkPS3EyePropExAuto = "auto_exposure";
 static constexpr char const* quirkPS3EyePropExValue = "exposure";
 static constexpr const int quirkPS3EyePropExAutoOn = 0;
 static constexpr const int quirkPS3EyePropExAutoOff = 1;
+static constexpr char const* quirkPiCameraPropExAuto = "auto_exposure";
+static constexpr char const* quirkPiCameraPropExValue =
+    "exposure_time_absolute";
+static constexpr const int quirkPiCameraPropExAutoOn = 0;
+static constexpr const int quirkPiCameraPropExAutoOff = 1;
 
 int UsbCameraImpl::RawToPercentage(const UsbCameraProperty& rawProp,
                                    int rawValue) {
@@ -1112,6 +1117,25 @@ void UsbCameraImpl::DeviceCacheVideoModes() {
     }
   }
 
+  // The Pi camera reports mode ranges, which we don't currently handle, so only
+  // provide a set of discrete modes; list based on
+  // https://picamera.readthedocs.io/en/release-1.10/fov.html
+  if (modes.empty() && m_picamera) {
+    for (VideoMode::PixelFormat pixelFormat :
+         {VideoMode::kYUYV, VideoMode::kMJPEG, VideoMode::kBGR}) {
+      modes.emplace_back(pixelFormat, 1920, 1080, 30);
+      modes.emplace_back(pixelFormat, 2592, 1944, 15);
+      modes.emplace_back(pixelFormat, 1296, 972, 42);
+      modes.emplace_back(pixelFormat, 1296, 730, 49);
+      modes.emplace_back(pixelFormat, 640, 480, 90);
+      modes.emplace_back(pixelFormat, 320, 240, 90);
+      modes.emplace_back(pixelFormat, 160, 120, 90);
+      modes.emplace_back(pixelFormat, 640, 480, 60);
+      modes.emplace_back(pixelFormat, 320, 240, 60);
+      modes.emplace_back(pixelFormat, 160, 120, 60);
+    }
+  }
+
   {
     std::scoped_lock lock(m_mutex);
     m_videoModes.swap(modes);
@@ -1192,6 +1216,7 @@ void UsbCameraImpl::SetQuirks() {
   wpi::StringRef desc = GetDescription(descbuf);
   m_lifecam_exposure =
       desc.endswith("LifeCam HD-3000") || desc.endswith("LifeCam Cinema (TM)");
+  m_picamera = desc.startswith("mmal service");
 
   int deviceNum = GetDeviceNum(m_path.c_str());
   if (deviceNum >= 0) {
@@ -1248,7 +1273,9 @@ void UsbCameraImpl::SetExposureAuto(CS_Status* status) {
   if (m_ps3eyecam_exposure) {
     SetProperty(GetPropertyIndex(quirkPS3EyePropExAuto),
                 quirkPS3EyePropExAutoOn, status);
-
+  } else if (m_picamera) {
+    SetProperty(GetPropertyIndex(quirkPiCameraPropExAuto),
+                quirkPiCameraPropExAutoOn, status);
   } else {
     SetProperty(GetPropertyIndex(kPropExAuto), 3, status);
   }
@@ -1258,6 +1285,9 @@ void UsbCameraImpl::SetExposureHoldCurrent(CS_Status* status) {
   if (m_ps3eyecam_exposure) {
     SetProperty(GetPropertyIndex(quirkPS3EyePropExAuto),
                 quirkPS3EyePropExAutoOff, status);  // manual
+  } else if (m_picamera) {
+    SetProperty(GetPropertyIndex(quirkPiCameraPropExAuto),
+                quirkPiCameraPropExAutoOff, status);  // manual
   } else {
     SetProperty(GetPropertyIndex(kPropExAuto), 1, status);  // manual
   }
@@ -1267,6 +1297,9 @@ void UsbCameraImpl::SetExposureManual(int value, CS_Status* status) {
   if (m_ps3eyecam_exposure) {
     SetProperty(GetPropertyIndex(quirkPS3EyePropExAuto),
                 quirkPS3EyePropExAutoOff, status);  // manual
+  } else if (m_picamera) {
+    SetProperty(GetPropertyIndex(quirkPiCameraPropExAuto),
+                quirkPiCameraPropExAutoOff, status);  // manual
   } else {
     SetProperty(GetPropertyIndex(kPropExAuto), 1, status);  // manual
   }
@@ -1277,6 +1310,8 @@ void UsbCameraImpl::SetExposureManual(int value, CS_Status* status) {
   }
   if (m_ps3eyecam_exposure) {
     SetProperty(GetPropertyIndex(quirkPS3EyePropExValue), value, status);
+  } else if (m_picamera) {
+    SetProperty(GetPropertyIndex(quirkPiCameraPropExValue), value, status);
   } else {
     SetProperty(GetPropertyIndex(kPropExValue), value, status);
   }

cscore/src/main/native/linux/UsbCameraImpl.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-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.                                                               */
@@ -166,6 +166,7 @@ class UsbCameraImpl : public SourceImpl {
   // Quirks
   bool m_lifecam_exposure{false};    // Microsoft LifeCam exposure
   bool m_ps3eyecam_exposure{false};  // PS3 Eyecam exposure
+  bool m_picamera{false};            // Raspberry Pi camera
 
   //
   // Variables protected by m_mutex

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/DutyCycle.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.                                                               */
@@ -75,6 +75,9 @@ void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle) {
   dutyCycleHandles->Free(dutyCycleHandle);
 }
 
+void HAL_SetDutyCycleSimDevice(HAL_EncoderHandle handle,
+                               HAL_SimDeviceHandle device) {}
+
 int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
                                   int32_t* status) {
   auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);

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/DriverStation.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-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.                                                               */
@@ -230,6 +230,10 @@ static int& GetThreadLocalLastCount() {
   return lastCount;
 }
 
+void HAL_WaitForCachedControlData(void) {
+  HAL_WaitForCachedControlDataTimeout(0);
+}
+
 HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) {
   int& lastCount = GetThreadLocalLastCount();
   std::unique_lock lock(newDSDataAvailableMutex);

hal/src/main/native/sim/PortsInternal.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-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.                                                               */
@@ -17,7 +17,7 @@ constexpr int32_t kNumAnalogOutputs = 2;
 constexpr int32_t kNumCounters = 8;
 constexpr int32_t kNumDigitalHeaders = 10;
 constexpr int32_t kNumPWMHeaders = 10;
-constexpr int32_t kNumDigitalChannels = 26;
+constexpr int32_t kNumDigitalChannels = 31;
 constexpr int32_t kNumPWMChannels = 20;
 constexpr int32_t kNumDigitalPWMOutputs = 6;
 constexpr int32_t kNumEncoders = 8;

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

hal/src/main/native/sim/mockdata/RoboRioData.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-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,17 +22,17 @@ void InitializeRoboRioData() {
 RoboRioData* hal::SimRoboRioData;
 void RoboRioData::ResetData() {
   fpgaButton.Reset(false);
-  vInVoltage.Reset(0.0);
+  vInVoltage.Reset(12.0);
   vInCurrent.Reset(0.0);
   userVoltage6V.Reset(6.0);
   userCurrent6V.Reset(0.0);
-  userActive6V.Reset(false);
+  userActive6V.Reset(true);
   userVoltage5V.Reset(5.0);
   userCurrent5V.Reset(0.0);
-  userActive5V.Reset(false);
+  userActive5V.Reset(true);
   userVoltage3V3.Reset(3.3);
   userCurrent3V3.Reset(0.0);
-  userActive3V3.Reset(false);
+  userActive3V3.Reset(true);
   userFaults6V.Reset(0);
   userFaults5V.Reset(0);
   userFaults3V3.Reset(0);

hal/src/main/native/sim/mockdata/RoboRioDataInternal.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-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,22 +30,22 @@ class RoboRioData {
 
  public:
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetFPGAButtonName> fpgaButton{false};
-  SimDataValue<double, HAL_MakeDouble, GetVInVoltageName> vInVoltage{0.0};
+  SimDataValue<double, HAL_MakeDouble, GetVInVoltageName> vInVoltage{12.0};
   SimDataValue<double, HAL_MakeDouble, GetVInCurrentName> vInCurrent{0.0};
   SimDataValue<double, HAL_MakeDouble, GetUserVoltage6VName> userVoltage6V{6.0};
   SimDataValue<double, HAL_MakeDouble, GetUserCurrent6VName> userCurrent6V{0.0};
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetUserActive6VName> userActive6V{
-      false};
+      true};
   SimDataValue<double, HAL_MakeDouble, GetUserVoltage5VName> userVoltage5V{5.0};
   SimDataValue<double, HAL_MakeDouble, GetUserCurrent5VName> userCurrent5V{0.0};
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetUserActive5VName> userActive5V{
-      false};
+      true};
   SimDataValue<double, HAL_MakeDouble, GetUserVoltage3V3Name> userVoltage3V3{
       3.3};
   SimDataValue<double, HAL_MakeDouble, GetUserCurrent3V3Name> userCurrent3V3{
       0.0};
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetUserActive3V3Name> userActive3V3{
-      false};
+      true};
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults6VName> userFaults6V{0};
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults5VName> userFaults5V{0};
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults3V3Name> userFaults3V3{0};

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)) {

simulation/halsim_gui/src/main/native/cpp/HALSimGui.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.                                                               */
@@ -301,6 +301,12 @@ bool HALSimGui::Initialize() {
   // Set initial window settings
   glfwWindowHint(GLFW_MAXIMIZED, gWindowMaximized ? GLFW_TRUE : GLFW_FALSE);
 
+  if (gWindowWidth == 0 || gWindowHeight == 0) {
+    gWindowWidth = 1280;
+    gWindowHeight = 720;
+    gWindowLoadedWidthHeight = false;
+  }
+
   float windowScale = 1.0;
   if (!gWindowLoadedWidthHeight) {
     glfwWindowHint(GLFW_SCALE_TO_MONITOR, GLFW_TRUE);

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-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.                                                               */
@@ -24,6 +24,7 @@ import edu.wpi.first.hal.HAL;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
@@ -35,7 +36,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
  * Subsystem#periodic()} methods to be called and for their default commands to be scheduled.
  */
 @SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods", "PMD.TooManyFields"})
-public final class CommandScheduler implements Sendable {
+public final class CommandScheduler implements Sendable, AutoCloseable {
   /**
    * The Singleton Instance.
    */
@@ -70,11 +71,6 @@ public final class CommandScheduler implements Sendable {
 
   private boolean m_disabled;
 
-  //NetworkTable entries for use in Sendable impl
-  private NetworkTableEntry m_namesEntry;
-  private NetworkTableEntry m_idsEntry;
-  private NetworkTableEntry m_cancelEntry;
-
   //Lists of user-supplied actions to be executed on scheduling events for every command.
   private final List<Consumer<Command>> m_initActions = new ArrayList<>();
   private final List<Consumer<Command>> m_executeActions = new ArrayList<>();
@@ -89,8 +85,22 @@ 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");
+    LiveWindow.setEnabledListener(() -> {
+      disable();
+      cancelAll();
+    });
+    LiveWindow.setDisabledListener(() -> {
+      enable();
+    });
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    LiveWindow.setEnabledListener(null);
+    LiveWindow.setDisabledListener(null);
   }
 
   /**
@@ -375,7 +385,7 @@ public final class CommandScheduler implements Sendable {
    * Cancels all commands that are currently scheduled.
    */
   public void cancelAll() {
-    for (Command command : m_scheduledCommands.keySet()) {
+    for (Command command : m_scheduledCommands.keySet().toArray(new Command[0])) {
       cancel(command);
     }
   }
@@ -473,12 +483,12 @@ public final class CommandScheduler implements Sendable {
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Scheduler");
-    m_namesEntry = builder.getEntry("Names");
-    m_idsEntry = builder.getEntry("Ids");
-    m_cancelEntry = builder.getEntry("Cancel");
+    final NetworkTableEntry namesEntry = builder.getEntry("Names");
+    final NetworkTableEntry idsEntry = builder.getEntry("Ids");
+    final NetworkTableEntry cancelEntry = builder.getEntry("Cancel");
     builder.setUpdateTable(() -> {
 
-      if (m_namesEntry == null || m_idsEntry == null || m_cancelEntry == null) {
+      if (namesEntry == null || idsEntry == null || cancelEntry == null) {
         return;
       }
 
@@ -489,21 +499,21 @@ public final class CommandScheduler implements Sendable {
         ids.put((double) command.hashCode(), command);
       }
 
-      double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]);
+      double[] toCancel = cancelEntry.getDoubleArray(new double[0]);
       if (toCancel.length > 0) {
         for (double hash : toCancel) {
           cancel(ids.get(hash));
           ids.remove(hash);
         }
-        m_cancelEntry.setDoubleArray(new double[0]);
+        cancelEntry.setDoubleArray(new double[0]);
       }
 
       List<String> names = new ArrayList<>();
 
       ids.values().forEach(command -> names.add(command.getName()));
 
-      m_namesEntry.setStringArray(names.toArray(new String[0]));
-      m_idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
+      namesEntry.setStringArray(names.toArray(new String[0]));
+      idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
     });
   }
 }

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/java/edu/wpi/first/wpilibj2/command/button/Button.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-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.                                                               */
@@ -23,7 +23,7 @@ import edu.wpi.first.wpilibj2.command.Subsystem;
  * wrapper around Trigger with the method names renamed to fit the Button object use.
  */
 @SuppressWarnings("PMD.TooManyMethods")
-public abstract class Button extends Trigger {
+public class Button extends Trigger {
   /**
    * Default constructor; creates a button that is never pressed (unless {@link Button#get()} is
    * overridden).

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.                                                               */
@@ -13,7 +13,7 @@
 using namespace frc2;
 
 CommandBase::CommandBase() {
-  frc::SendableRegistry::GetInstance().AddLW(this, GetTypeName(*this));
+  frc::SendableRegistry::GetInstance().Add(this, GetTypeName(*this));
 }
 
 void CommandBase::AddRequirements(
@@ -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.                                                               */
@@ -9,11 +9,14 @@
 
 #include <frc/RobotState.h>
 #include <frc/WPIErrors.h>
+#include <frc/livewindow/LiveWindow.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>
+#include <wpi/SmallVector.h>
 
 #include "frc2/command/CommandGroupBase.h"
 #include "frc2/command/CommandState.h"
@@ -41,11 +44,6 @@ class CommandScheduler::Impl {
 
   bool disabled{false};
 
-  // NetworkTable entries for use in Sendable impl
-  nt::NetworkTableEntry namesEntry;
-  nt::NetworkTableEntry idsEntry;
-  nt::NetworkTableEntry cancelEntry;
-
   // Lists of user-supplied actions to be executed on scheduling events for
   // every command.
   wpi::SmallVector<Action, 4> initActions;
@@ -67,10 +65,23 @@ 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");
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = [this] {
+    this->Disable();
+    this->CancelAll();
+  };
+  scheduler->disabled = [this] { this->Enable(); };
 }
 
-CommandScheduler::~CommandScheduler() {}
+CommandScheduler::~CommandScheduler() {
+  frc::SendableRegistry::GetInstance().Remove(this);
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = nullptr;
+  scheduler->disabled = nullptr;
+}
 
 CommandScheduler& CommandScheduler::GetInstance() {
   static CommandScheduler scheduler;
@@ -245,6 +256,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 +269,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()) {
@@ -294,9 +318,11 @@ void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
 }
 
 void CommandScheduler::CancelAll() {
+  wpi::SmallVector<Command*, 16> commands;
   for (auto&& command : m_impl->scheduledCommands) {
-    Cancel(command.first);
+    commands.emplace_back(command.first);
   }
+  Cancel(commands);
 }
 
 double CommandScheduler::TimeSinceScheduled(const Command* command) const {
@@ -363,14 +389,14 @@ void CommandScheduler::OnCommandFinish(Action action) {
 
 void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Scheduler");
-  m_impl->namesEntry = builder.GetEntry("Names");
-  m_impl->idsEntry = builder.GetEntry("Ids");
-  m_impl->cancelEntry = builder.GetEntry("Cancel");
+  auto namesEntry = builder.GetEntry("Names");
+  auto idsEntry = builder.GetEntry("Ids");
+  auto cancelEntry = builder.GetEntry("Cancel");
 
-  builder.SetUpdateTable([this] {
+  builder.SetUpdateTable([=] {
     double tmp[1];
     tmp[0] = 0;
-    auto toCancel = m_impl->cancelEntry.GetDoubleArray(tmp);
+    auto toCancel = cancelEntry.GetDoubleArray(tmp);
     for (auto cancel : toCancel) {
       uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
       Command* command = reinterpret_cast<Command*>(ptrTmp);
@@ -378,7 +404,8 @@ void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
           m_impl->scheduledCommands.end()) {
         Cancel(command);
       }
-      m_impl->cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
+      nt::NetworkTableEntry(cancelEntry)
+          .SetDoubleArray(wpi::ArrayRef<double>{});
     }
 
     wpi::SmallVector<std::string, 8> names;
@@ -388,8 +415,8 @@ void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
       uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
       ids.emplace_back(static_cast<double>(ptrTmp));
     }
-    m_impl->namesEntry.SetStringArray(names);
-    m_impl->idsEntry.SetDoubleArray(ids);
+    nt::NetworkTableEntry(namesEntry).SetStringArray(names);
+    nt::NetworkTableEntry(idsEntry).SetDoubleArray(ids);
   });
 }
 

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.                                                               */
@@ -15,8 +15,8 @@ Trigger::Trigger(const Trigger& other) : m_isActive(other.m_isActive) {}
 
 Trigger Trigger::WhenActive(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (!pressedLast && pressed) {
           command->Schedule(interruptible);
@@ -30,13 +30,19 @@ 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));
 }
 
 Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (pressed) {
           command->Schedule(interruptible);
@@ -52,13 +58,20 @@ 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));
 }
 
 Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (!pressedLast && pressed) {
           command->Schedule(interruptible);
@@ -73,8 +86,8 @@ Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
 
 Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (pressedLast && !pressed) {
           command->Schedule(interruptible);
@@ -87,13 +100,19 @@ 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));
 }
 
 Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (!pressedLast && pressed) {
           if (command->IsScheduled()) {
@@ -110,8 +129,8 @@ Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
 
 Trigger Trigger::CancelWhenActive(Command* command) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command]() mutable {
+        bool pressed = m_isActive();
 
         if (!pressedLast && pressed) {
           command->Cancel();

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/JoystickButton.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.                                                               */
@@ -26,12 +26,8 @@ class JoystickButton : public Button {
    * @param buttonNumber The number of the button on the joystic.
    */
   explicit JoystickButton(frc::GenericHID* joystick, int buttonNumber)
-      : m_joystick{joystick}, m_buttonNumber{buttonNumber} {}
-
-  bool Get() const override { return m_joystick->GetRawButton(m_buttonNumber); }
-
- private:
-  frc::GenericHID* m_joystick;
-  int m_buttonNumber;
+      : Button([joystick, buttonNumber] {
+          return joystick->GetRawButton(buttonNumber);
+        }) {}
 };
 }  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.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.                                                               */
@@ -27,15 +27,8 @@ class POVButton : public Button {
    * @param povNumber The number of the POV on the joystick.
    */
   POVButton(frc::GenericHID* joystick, int angle, int povNumber = 0)
-      : m_joystick{joystick}, m_angle{angle}, m_povNumber{povNumber} {}
-
-  bool Get() const override {
-    return m_joystick->GetPOV(m_povNumber) == m_angle;
-  }
-
- private:
-  frc::GenericHID* m_joystick;
-  int m_angle;
-  int m_povNumber;
+      : Button([joystick, angle, povNumber] {
+          joystick->GetPOV(povNumber) == angle;
+        }) {}
 };
 }  // namespace frc2

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"
 
@@ -44,13 +47,6 @@ class Trigger {
 
   Trigger(const Trigger& other);
 
-  /**
-   * Returns whether the trigger is active.  Can be overridden by a subclass.
-   *
-   * @return Whether the trigger is active.
-   */
-  virtual bool Get() const { return m_isActive(); }
-
   /**
    * Binds a command to start when the trigger becomes active.  Takes a
    * raw pointer, and so is non-owning; users are responsible for the lifespan
@@ -76,11 +72,11 @@ class Trigger {
                          Command, std::remove_reference_t<T>>>>
   Trigger WhenActive(T&& command, bool interruptible = true) {
     CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
+        [pressedLast = m_isActive(), *this,
          command = std::make_unique<std::remove_reference_t<T>>(
              std::forward<T>(command)),
          interruptible]() mutable {
-          bool pressed = Get();
+          bool pressed = m_isActive();
 
           if (!pressedLast && pressed) {
             command->Schedule(interruptible);
@@ -99,7 +95,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
@@ -126,11 +131,11 @@ class Trigger {
                          Command, std::remove_reference_t<T>>>>
   Trigger WhileActiveContinous(T&& command, bool interruptible = true) {
     CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
+        [pressedLast = m_isActive(), *this,
          command = std::make_unique<std::remove_reference_t<T>>(
              std::forward<T>(command)),
          interruptible]() mutable {
-          bool pressed = Get();
+          bool pressed = m_isActive();
 
           if (pressed) {
             command->Schedule(interruptible);
@@ -149,9 +154,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
@@ -178,11 +191,11 @@ class Trigger {
                          Command, std::remove_reference_t<T>>>>
   Trigger WhileActiveOnce(T&& command, bool interruptible = true) {
     CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
+        [pressedLast = m_isActive(), *this,
          command = std::make_unique<std::remove_reference_t<T>>(
              std::forward<T>(command)),
          interruptible]() mutable {
-          bool pressed = Get();
+          bool pressed = m_isActive();
 
           if (!pressedLast && pressed) {
             command->Schedule(interruptible);
@@ -220,11 +233,11 @@ class Trigger {
                          Command, std::remove_reference_t<T>>>>
   Trigger WhenInactive(T&& command, bool interruptible = true) {
     CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
+        [pressedLast = m_isActive(), *this,
          command = std::make_unique<std::remove_reference_t<T>>(
              std::forward<T>(command)),
          interruptible]() mutable {
-          bool pressed = Get();
+          bool pressed = m_isActive();
 
           if (pressedLast && !pressed) {
             command->Schedule(interruptible);
@@ -242,7 +255,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
@@ -269,11 +291,11 @@ class Trigger {
                          Command, std::remove_reference_t<T>>>>
   Trigger ToggleWhenActive(T&& command, bool interruptible = true) {
     CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
+        [pressedLast = m_isActive(), *this,
          command = std::make_unique<std::remove_reference_t<T>>(
              std::forward<T>(command)),
          interruptible]() mutable {
-          bool pressed = Get();
+          bool pressed = m_isActive();
 
           if (!pressedLast && pressed) {
             if (command->IsScheduled()) {
@@ -304,7 +326,7 @@ class Trigger {
    * @return A trigger which is active when both component triggers are active.
    */
   Trigger operator&&(Trigger rhs) {
-    return Trigger([*this, rhs] { return Get() && rhs.Get(); });
+    return Trigger([*this, rhs] { return m_isActive() && rhs.m_isActive(); });
   }
 
   /**
@@ -313,7 +335,7 @@ class Trigger {
    * @return A trigger which is active when either component trigger is active.
    */
   Trigger operator||(Trigger rhs) {
-    return Trigger([*this, rhs] { return Get() || rhs.Get(); });
+    return Trigger([*this, rhs] { return m_isActive() || rhs.m_isActive(); });
   }
 
   /**
@@ -323,7 +345,7 @@ class Trigger {
    * and vice-versa.
    */
   Trigger operator!() {
-    return Trigger([*this] { return !Get(); });
+    return Trigger([*this] { return !m_isActive(); });
   }
 
  private:

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-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.                                                               */

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-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.                                                               */
@@ -54,4 +54,22 @@ class SchedulerTest extends CommandTestBase {
     scheduler.registerSubsystem(system);
     assertDoesNotThrow(() -> scheduler.unregisterSubsystem(system));
   }
+
+  @Test
+  void schedulerCancelAllTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    Counter counter = new Counter();
+
+    scheduler.onCommandInterrupt(command -> counter.increment());
+
+    Command command = new WaitCommand(10);
+    Command command2 = new WaitCommand(10);
+
+    scheduler.schedule(command);
+    scheduler.schedule(command2);
+    scheduler.cancelAll();
+
+    assertEquals(counter.m_counter, 2);
+  }
 }

wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.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.                                                               */
@@ -54,3 +54,21 @@ TEST_F(SchedulerTest, UnregisterSubsystemTest) {
 
   EXPECT_NO_FATAL_FAILURE(scheduler.UnregisterSubsystem(&system));
 }
+
+TEST_F(SchedulerTest, SchedulerCancelAllTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  RunCommand command([] {}, {});
+  RunCommand command2([] {}, {});
+
+  int counter = 0;
+
+  scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; });
+
+  scheduler.Schedule(&command);
+  scheduler.Schedule(&command2);
+  scheduler.Run();
+  scheduler.CancelAll();
+
+  EXPECT_EQ(counter, 2);
+}

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-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.                                                               */
@@ -17,6 +17,7 @@ import edu.wpi.first.hal.HAL;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.Sendable;
 import edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
@@ -81,9 +82,6 @@ public final class Scheduler implements Sendable, AutoCloseable {
    */
   @SuppressWarnings({"PMD.LooseCoupling", "PMD.UseArrayListInsteadOfVector"})
   private final Vector<Command> m_additions = new Vector<>();
-  private NetworkTableEntry m_namesEntry;
-  private NetworkTableEntry m_idsEntry;
-  private NetworkTableEntry m_cancelEntry;
   /**
    * A list of all {@link edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler Buttons}. It is
    * created lazily.
@@ -98,11 +96,20 @@ public final class Scheduler implements Sendable, AutoCloseable {
   private Scheduler() {
     HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
     SendableRegistry.addLW(this, "Scheduler");
+    LiveWindow.setEnabledListener(() -> {
+      disable();
+      removeAll();
+    });
+    LiveWindow.setDisabledListener(() -> {
+      enable();
+    });
   }
 
   @Override
   public void close() {
     SendableRegistry.remove(this);
+    LiveWindow.setEnabledListener(null);
+    LiveWindow.setDisabledListener(null);
   }
 
   /**
@@ -319,13 +326,13 @@ public final class Scheduler implements Sendable, AutoCloseable {
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Scheduler");
-    m_namesEntry = builder.getEntry("Names");
-    m_idsEntry = builder.getEntry("Ids");
-    m_cancelEntry = builder.getEntry("Cancel");
+    final NetworkTableEntry namesEntry = builder.getEntry("Names");
+    final NetworkTableEntry idsEntry = builder.getEntry("Ids");
+    final NetworkTableEntry cancelEntry = builder.getEntry("Cancel");
     builder.setUpdateTable(() -> {
-      if (m_namesEntry != null && m_idsEntry != null && m_cancelEntry != null) {
+      if (namesEntry != null && idsEntry != null && cancelEntry != null) {
         // Get the commands to cancel
-        double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]);
+        double[] toCancel = cancelEntry.getDoubleArray(new double[0]);
         if (toCancel.length > 0) {
           for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
             for (double d : toCancel) {
@@ -334,7 +341,7 @@ public final class Scheduler implements Sendable, AutoCloseable {
               }
             }
           }
-          m_cancelEntry.setDoubleArray(new double[0]);
+          cancelEntry.setDoubleArray(new double[0]);
         }
 
         if (m_runningCommandsChanged) {
@@ -351,8 +358,8 @@ public final class Scheduler implements Sendable, AutoCloseable {
             ids[number] = e.getData().hashCode();
             number++;
           }
-          m_namesEntry.setStringArray(commands);
-          m_idsEntry.setDoubleArray(ids);
+          namesEntry.setStringArray(commands);
+          idsEntry.setDoubleArray(ids);
         }
       }
     });

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/Scheduler.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-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,7 @@
 #include "frc/buttons/ButtonScheduler.h"
 #include "frc/commands/Command.h"
 #include "frc/commands/Subsystem.h"
+#include "frc/livewindow/LiveWindow.h"
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
 
@@ -198,9 +199,20 @@ Scheduler::Scheduler() : m_impl(new Impl) {
   HAL_Report(HALUsageReporting::kResourceType_Command,
              HALUsageReporting::kCommand_Scheduler);
   SendableRegistry::GetInstance().AddLW(this, "Scheduler");
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = [this] {
+    this->SetEnabled(false);
+    this->RemoveAll();
+  };
+  scheduler->disabled = [this] { this->SetEnabled(true); };
 }
 
-Scheduler::~Scheduler() {}
+Scheduler::~Scheduler() {
+  SendableRegistry::GetInstance().Remove(this);
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = nullptr;
+  scheduler->disabled = nullptr;
+}
 
 void Scheduler::Impl::Remove(Command* command) {
   if (!commands.erase(command)) return;

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());
+}