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
 }

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.

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

docs/build.gradle

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

hal/src/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/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/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};

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<>();
@@ -91,6 +87,20 @@ public final class CommandScheduler implements Sendable {
   CommandScheduler() {
     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/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.                                                               */
@@ -93,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/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,12 +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"
@@ -42,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;
@@ -71,9 +68,20 @@ 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;
@@ -248,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) {
@@ -255,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()) {
@@ -297,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 {
@@ -366,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);
@@ -381,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;
@@ -391,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/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/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/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.                                                               */
@@ -62,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"
@@ -42,7 +43,21 @@ class TrapezoidProfileCommand
    */
   TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
                           std::function<void(State)> output,
-                          std::initializer_list<Subsystem*> requirements = {})
+                          std::initializer_list<Subsystem*> requirements)
+      : m_profile(profile), m_output(output) {
+    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);
   }

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/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;

wpilibc/src/main/native/cpp/SensorUtil.cpp

@@ -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.                                                               */
@@ -21,6 +21,7 @@ using namespace frc;
 
 const int SensorUtil::kDigitalChannels = HAL_GetNumDigitalChannels();
 const int SensorUtil::kAnalogInputs = HAL_GetNumAnalogInputs();
+const int SensorUtil::kAnalogOutputs = HAL_GetNumAnalogOutputs();
 const int SensorUtil::kSolenoidChannels = HAL_GetNumSolenoidChannels();
 const int SensorUtil::kSolenoidModules = HAL_GetNumPCMModules();
 const int SensorUtil::kPwmChannels = HAL_GetNumPWMChannels();

wpilibc/src/main/native/cpp/XboxController.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.                                                               */
@@ -17,25 +17,25 @@ XboxController::XboxController(int port) : GenericHID(port) {
 
 double XboxController::GetX(JoystickHand hand) const {
   if (hand == kLeftHand) {
-    return GetRawAxis(0);
+    return GetRawAxis(static_cast<int>(Axis::kLeftX));
   } else {
-    return GetRawAxis(4);
+    return GetRawAxis(static_cast<int>(Axis::kRightX));
   }
 }
 
 double XboxController::GetY(JoystickHand hand) const {
   if (hand == kLeftHand) {
-    return GetRawAxis(1);
+    return GetRawAxis(static_cast<int>(Axis::kLeftY));
   } else {
-    return GetRawAxis(5);
+    return GetRawAxis(static_cast<int>(Axis::kRightY));
   }
 }
 
 double XboxController::GetTriggerAxis(JoystickHand hand) const {
   if (hand == kLeftHand) {
-    return GetRawAxis(2);
+    return GetRawAxis(static_cast<int>(Axis::kLeftTrigger));
   } else {
-    return GetRawAxis(3);
+    return GetRawAxis(static_cast<int>(Axis::kRightTrigger));
   }
 }
 

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

@@ -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.                                                               */
@@ -26,6 +26,12 @@ PIDController::PIDController(double Kp, double Ki, double Kd,
   frc::SendableRegistry::GetInstance().Add(this, "PIDController", instances);
 }
 
+void PIDController::SetPID(double Kp, double Ki, double Kd) {
+  m_Kp = Kp;
+  m_Ki = Ki;
+  m_Kd = Kd;
+}
+
 void PIDController::SetP(double Kp) { m_Kp = Kp; }
 
 void PIDController::SetI(double Ki) { m_Ki = Ki; }

wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2012-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2012-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.                                                               */
@@ -104,10 +104,12 @@ void LiveWindow::SetEnabled(bool enabled) {
   // Force table generation now to make sure everything is defined
   UpdateValuesUnsafe();
   if (enabled) {
+    if (this->enabled) this->enabled();
   } else {
     m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
       cbdata.builder.StopLiveWindowMode();
     });
+    if (this->disabled) this->disabled();
   }
   m_impl->enabledEntry.SetBoolean(enabled);
 }

wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.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.                                                               */
@@ -32,7 +32,9 @@ void SendableBuilderImpl::UpdateTable() {
   for (auto& property : m_properties) {
     if (property.update) property.update(property.entry, time);
   }
-  if (m_updateTable) m_updateTable();
+  for (auto& updateTable : m_updateTables) {
+    updateTable();
+  }
 }
 
 void SendableBuilderImpl::StartListeners() {
@@ -71,7 +73,7 @@ void SendableBuilderImpl::SetSafeState(std::function<void()> func) {
 }
 
 void SendableBuilderImpl::SetUpdateTable(std::function<void()> func) {
-  m_updateTable = func;
+  m_updateTables.emplace_back(std::move(func));
 }
 
 nt::NetworkTableEntry SendableBuilderImpl::GetEntry(const wpi::Twine& key) {

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

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

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

@@ -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.                                                               */
@@ -25,6 +25,7 @@
 #include "frc/WPIErrors.h"
 #include "frc/livewindow/LiveWindow.h"
 #include "frc/smartdashboard/SmartDashboard.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
 
 typedef void (*SetCameraServerSharedFP)(frc::CameraServerShared* shared);
 
@@ -121,6 +122,8 @@ RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
   m_threadId = std::this_thread::get_id();
 
   SetupCameraServerShared();
+  TrajectoryGenerator::SetErrorHandler(
+      [](const char* error) { DriverStation::ReportError(error); });
 
   auto inst = nt::NetworkTableInstance::GetDefault();
   inst.SetNetworkIdentity("Robot");

wpilibc/src/main/native/include/frc/AddressableLED.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.                                                               */
@@ -22,6 +22,8 @@ namespace frc {
 
 /**
  * A class for driving addressable LEDs, such as WS2812s and NeoPixels.
+ *
+ * <p>Only 1 LED driver is currently supported by the roboRIO.
  */
 class AddressableLED : public ErrorBase {
  public:
@@ -83,7 +85,7 @@ class AddressableLED : public ErrorBase {
   /**
    * Constructs a new driver for a specific port.
    *
-   * @param port the output port to use (Must be a PWM port)
+   * @param port the output port to use (Must be a PWM header)
    */
   explicit AddressableLED(int port);
 
@@ -95,6 +97,8 @@ class AddressableLED : public ErrorBase {
    * <p>Calling this is an expensive call, so its best to call it once, then
    * just update data.
    *
+   * <p>The max length is 5460 LEDs.
+   *
    * @param length the strip length
    */
   void SetLength(int length);

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

@@ -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.                                                               */
@@ -100,17 +100,6 @@ class AnalogTrigger : public ErrorBase,
    */
   void SetAveraged(bool useAveragedValue);
 
-  /**
-   * Configure the analog trigger to use the duty cycle vs. raw values.
-   *
-   * If the value is true, then the duty cycle value is selected for the analog
-   * trigger, otherwise the immediate value is used.
-   *
-   * @param useDutyCycle If true, use the duty cycle value, otherwise use the
-   *                         instantaneous reading
-   */
-  void SetDutyCycle(bool useDutyCycle);
-
   /**
    * Configure the analog trigger to use a filtered value.
    *

wpilibc/src/main/native/include/frc/IterativeRobotBase.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.                                                               */
@@ -138,7 +138,6 @@ class IterativeRobotBase : public RobotBase {
    */
   virtual void TestPeriodic();
 
- protected:
   /**
    * Constructor for IterativeRobotBase.
    *
@@ -156,6 +155,7 @@ class IterativeRobotBase : public RobotBase {
 
   virtual ~IterativeRobotBase() = default;
 
+ protected:
   IterativeRobotBase(IterativeRobotBase&&) = default;
   IterativeRobotBase& operator=(IterativeRobotBase&&) = default;
 

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

@@ -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.                                                               */
@@ -176,7 +176,6 @@ class RobotBase {
 
   static constexpr bool IsSimulation() { return !IsReal(); }
 
- protected:
   /**
    * Constructor for a generic robot program.
    *
@@ -192,6 +191,7 @@ class RobotBase {
 
   virtual ~RobotBase();
 
+ protected:
   // m_ds isn't moved in these because DriverStation is a singleton; every
   // instance of RobotBase has a reference to the same object.
   RobotBase(RobotBase&&) noexcept;

wpilibc/src/main/native/include/frc/XboxController.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.                                                               */
@@ -244,6 +244,15 @@ class XboxController : public GenericHID {
     kBack = 7,
     kStart = 8
   };
+
+  enum class Axis {
+    kLeftX = 0,
+    kRightX = 4,
+    kLeftY = 1,
+    kRightY = 5,
+    kLeftTrigger = 2,
+    kRightTrigger = 3
+  };
 };
 
 }  // namespace frc

wpilibc/src/main/native/include/frc/controller/ArmFeedforward.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.                                                               */
@@ -16,6 +16,7 @@ namespace frc {
  * a motor acting against the force of gravity on a beam suspended at an angle).
  */
 class ArmFeedforward {
+ public:
   using Angle = units::radians;
   using Velocity = units::radians_per_second;
   using Acceleration = units::compound_unit<units::radians_per_second,
@@ -26,7 +27,6 @@ class ArmFeedforward {
   using ka_unit =
       units::compound_unit<units::volts, units::inverse<Acceleration>>;
 
- public:
   constexpr ArmFeedforward() = default;
 
   /**

wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.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.                                                               */
@@ -17,6 +17,7 @@ namespace frc {
  */
 template <class Distance>
 class ElevatorFeedforward {
+ public:
   using Velocity =
       units::compound_unit<Distance, units::inverse<units::seconds>>;
   using Acceleration =
@@ -25,7 +26,6 @@ class ElevatorFeedforward {
   using ka_unit =
       units::compound_unit<units::volts, units::inverse<Acceleration>>;
 
- public:
   ElevatorFeedforward() = default;
 
   /**

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -33,6 +33,7 @@ template <class Distance>
 class ProfiledPIDController
     : public Sendable,
       public SendableHelper<ProfiledPIDController<Distance>> {
+ public:
   using Distance_t = units::unit_t<Distance>;
   using Velocity =
       units::compound_unit<Distance, units::inverse<units::seconds>>;
@@ -43,10 +44,10 @@ class ProfiledPIDController
   using State = typename TrapezoidProfile<Distance>::State;
   using Constraints = typename TrapezoidProfile<Distance>::Constraints;
 
- public:
   /**
    * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
-   * Kd.
+   * Kd. Users should call reset() when they first start running the controller
+   * to avoid unwanted behavior.
    *
    * @param Kp          The proportional coefficient.
    * @param Ki          The integral coefficient.
@@ -292,9 +293,34 @@ class ProfiledPIDController
   }
 
   /**
-   * Reset the previous error, the integral term, and disable the controller.
+   * Reset the previous error and the integral term.
+   *
+   * @param measurement The current measured State of the system.
    */
-  void Reset() { m_controller.Reset(); }
+  void Reset(const State& measurement) {
+    m_controller.Reset();
+    m_setpoint = measurement;
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system.
+   * @param measuredVelocity The current measured velocity of the system.
+   */
+  void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) {
+    Reset(State{measuredPosition, measuredVelocity});
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system. The
+   * velocity is assumed to be zero.
+   */
+  void Reset(Distance_t measuredPosition) {
+    Reset(measuredPosition, Velocity_t(0));
+  }
 
   void InitSendable(frc::SendableBuilder& builder) override {
     builder.SetSmartDashboardType("ProfiledPIDController");

wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.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.                                                               */
@@ -17,6 +17,7 @@ namespace frc {
  */
 template <class Distance>
 class SimpleMotorFeedforward {
+ public:
   using Velocity =
       units::compound_unit<Distance, units::inverse<units::seconds>>;
   using Acceleration =
@@ -25,7 +26,6 @@ class SimpleMotorFeedforward {
   using ka_unit =
       units::compound_unit<units::volts, units::inverse<Acceleration>>;
 
- public:
   constexpr SimpleMotorFeedforward() = default;
 
   /**

wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2012-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2012-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 @@
 
 #pragma once
 
+#include <functional>
 #include <memory>
 
 namespace frc {
@@ -22,6 +23,9 @@ class LiveWindow {
   LiveWindow(const LiveWindow&) = delete;
   LiveWindow& operator=(const LiveWindow&) = delete;
 
+  std::function<void()> enabled;
+  std::function<void()> disabled;
+
   /**
    * Get an instance of the LiveWindow main class.
    *

wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.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.                                                               */
@@ -202,7 +202,7 @@ class SendableBuilderImpl : public SendableBuilder {
 
   std::vector<Property> m_properties;
   std::function<void()> m_safeState;
-  std::function<void()> m_updateTable;
+  std::vector<std::function<void()>> m_updateTables;
   std::shared_ptr<nt::NetworkTable> m_table;
   nt::NetworkTableEntry m_controllableEntry;
   bool m_actuator = false;

wpilibc/src/main/native/include/frc/spline/SplineParameterizer.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.                                                               */
@@ -33,10 +33,13 @@
 
 #include <frc/spline/Spline.h>
 
+#include <stack>
+#include <string>
 #include <utility>
 #include <vector>
 
 #include <units/units.h>
+#include <wpi/Twine.h>
 
 namespace frc {
 
@@ -47,6 +50,11 @@ class SplineParameterizer {
  public:
   using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
 
+  struct MalformedSplineException : public std::runtime_error {
+    explicit MalformedSplineException(const char* what_arg)
+        : runtime_error(what_arg) {}
+  };
+
   /**
    * Parameterizes the spline. This method breaks up the spline into various
    * arcs until their dx, dy, and dtheta are within specific tolerances.
@@ -64,14 +72,48 @@ class SplineParameterizer {
   static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
                                                      double t0 = 0.0,
                                                      double t1 = 1.0) {
-    std::vector<PoseWithCurvature> arr;
+    std::vector<PoseWithCurvature> splinePoints;
 
-    // The parameterization does not add the first initial point. Let's add
-    // that.
-    arr.push_back(spline.GetPoint(t0));
+    // The parameterization does not add the initial point. Let's add that.
+    splinePoints.push_back(spline.GetPoint(t0));
 
-    GetSegmentArc(spline, &arr, t0, t1);
-    return arr;
+    // We use an "explicit stack" to simulate recursion, instead of a recursive
+    // function call This give us greater control, instead of a stack overflow
+    std::stack<StackContents> stack;
+    stack.emplace(StackContents{t0, t1});
+
+    StackContents current;
+    PoseWithCurvature start;
+    PoseWithCurvature end;
+    int iterations = 0;
+
+    while (!stack.empty()) {
+      current = stack.top();
+      stack.pop();
+      start = spline.GetPoint(current.t0);
+      end = spline.GetPoint(current.t1);
+
+      const auto twist = start.first.Log(end.first);
+
+      if (units::math::abs(twist.dy) > kMaxDy ||
+          units::math::abs(twist.dx) > kMaxDx ||
+          units::math::abs(twist.dtheta) > kMaxDtheta) {
+        stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1});
+        stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2});
+      } else {
+        splinePoints.push_back(spline.GetPoint(current.t1));
+      }
+
+      if (iterations++ >= kMaxIterations) {
+        throw MalformedSplineException(
+            "Could not parameterize a malformed spline. "
+            "This means that you probably had two or more adjacent "
+            "waypoints that were very close together with headings "
+            "in opposing directions.");
+      }
+    }
+
+    return splinePoints;
   }
 
  private:
@@ -80,33 +122,19 @@ class SplineParameterizer {
   static constexpr units::meter_t kMaxDy = 0.05_in;
   static constexpr units::radian_t kMaxDtheta = 0.0872_rad;
 
+  struct StackContents {
+    double t0;
+    double t1;
+  };
+
   /**
-   * Breaks up the spline into arcs until the dx, dy, and theta of each arc is
-   * within tolerance.
-   *
-   * @param spline The spline to parameterize.
-   * @param vector Pointer to vector of poses.
-   * @param t0 Starting point for arc.
-   * @param t1 Ending point for arc.
+   * A malformed spline does not actually explode the LIFO stack size. Instead,
+   * the stack size stays at a relatively small number (e.g. 30) and never
+   * decreases. Because of this, we must count iterations. Even long, complex
+   * paths don't usually go over 300 iterations, so hitting this maximum should
+   * definitely indicate something has gone wrong.
    */
-  template <int Dim>
-  static void GetSegmentArc(const Spline<Dim>& spline,
-                            std::vector<PoseWithCurvature>* vector, double t0,
-                            double t1) {
-    const auto start = spline.GetPoint(t0);
-    const auto end = spline.GetPoint(t1);
-
-    const auto twist = start.first.Log(end.first);
-
-    if (units::math::abs(twist.dy) > kMaxDy ||
-        units::math::abs(twist.dx) > kMaxDx ||
-        units::math::abs(twist.dtheta) > kMaxDtheta) {
-      GetSegmentArc(spline, vector, t0, (t0 + t1) / 2);
-      GetSegmentArc(spline, vector, (t0 + t1) / 2, t1);
-    } else {
-      vector->push_back(spline.GetPoint(t1));
-    }
-  }
+  static constexpr int kMaxIterations = 5000;
 
   friend class CubicHermiteSplineTest;
   friend class QuinticHermiteSplineTest;

wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.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 @@
 
 #pragma once
 
+#include <functional>
 #include <memory>
 #include <utility>
 #include <vector>
@@ -113,5 +114,20 @@ class TrajectoryGenerator {
     }
     return splinePoints;
   }
+
+  /**
+   * Set error reporting function. By default, it is output to stderr.
+   *
+   * @param func Error reporting function.
+   */
+  static void SetErrorHandler(std::function<void(const char*)> func) {
+    s_errorFunc = std::move(func);
+  }
+
+ private:
+  static void ReportError(const char* error);
+
+  static const Trajectory kDoNothingTrajectory;
+  static std::function<void(const char*)> s_errorFunc;
 };
 }  // namespace frc

wpilibc/src/main/native/include/frc/util/Color.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.                                                               */
@@ -399,7 +399,7 @@ class Color {
   /**
    * #20B2AA.
    */
-  static const Color kLightSeagGeen;
+  static const Color kLightSeaGreen;
 
   /**
    * #87CEFA.
@@ -414,7 +414,7 @@ class Color {
   /**
    * #B0C4DE.
    */
-  static const Color kLightSteellue;
+  static const Color kLightSteelBlue;
 
   /**
    * #FFFFE0.
@@ -864,13 +864,13 @@ inline constexpr Color Color::kLightGray{0.827451f, 0.827451f, 0.827451f};
 inline constexpr Color Color::kLightGreen{0.5647059f, 0.93333334f, 0.5647059f};
 inline constexpr Color Color::kLightPink{1.0f, 0.7137255f, 0.75686276f};
 inline constexpr Color Color::kLightSalmon{1.0f, 0.627451f, 0.47843137f};
-inline constexpr Color Color::kLightSeagGeen{0.1254902f, 0.69803923f,
+inline constexpr Color Color::kLightSeaGreen{0.1254902f, 0.69803923f,
                                              0.6666667f};
 inline constexpr Color Color::kLightSkyBlue{0.5294118f, 0.80784315f,
                                             0.98039216f};
 inline constexpr Color Color::kLightSlateGray{0.46666667f, 0.53333336f, 0.6f};
-inline constexpr Color Color::kLightSteellue{0.6901961f, 0.76862746f,
-                                             0.87058824f};
+inline constexpr Color Color::kLightSteelBlue{0.6901961f, 0.76862746f,
+                                              0.87058824f};
 inline constexpr Color Color::kLightYellow{1.0f, 1.0f, 0.8784314f};
 inline constexpr Color Color::kLime{0.0f, 1.0f, 0.0f};
 inline constexpr Color Color::kLimeGreen{0.19607843f, 0.8039216f, 0.19607843f};

wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.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.                                                               */
@@ -120,3 +120,14 @@ TEST_F(CubicHermiteSplineTest, OneInterior) {
   Pose2d end{4_m, 0_m, 0_rad};
   Run(start, waypoints, end);
 }
+
+TEST_F(CubicHermiteSplineTest, ThrowsOnMalformed) {
+  EXPECT_THROW(
+      Run(Pose2d{0_m, 0_m, Rotation2d(0_deg)}, std::vector<Translation2d>{},
+          Pose2d{1_m, 0_m, Rotation2d(180_deg)}),
+      SplineParameterizer::MalformedSplineException);
+  EXPECT_THROW(
+      Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector<Translation2d>{},
+          Pose2d{10_m, 11_m, Rotation2d(-90_deg)}),
+      SplineParameterizer::MalformedSplineException);
+}

wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.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.                                                               */
@@ -85,3 +85,12 @@ TEST_F(QuinticHermiteSplineTest, SquigglyCurve) {
   Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)),
       Pose2d(-1_m, 0_m, Rotation2d(90_deg)));
 }
+
+TEST_F(QuinticHermiteSplineTest, ThrowsOnMalformed) {
+  EXPECT_THROW(Run(Pose2d(0_m, 0_m, Rotation2d(0_deg)),
+                   Pose2d(1_m, 0_m, Rotation2d(180_deg))),
+               SplineParameterizer::MalformedSplineException);
+  EXPECT_THROW(Run(Pose2d(10_m, 10_m, Rotation2d(90_deg)),
+                   Pose2d(10_m, 11_m, Rotation2d(-90_deg))),
+               SplineParameterizer::MalformedSplineException);
+}

wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.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.                                                               */
@@ -33,3 +33,13 @@ TEST(TrajectoryGenerationTest, ObeysConstraints) {
                 12_fps_sq + 0.01_fps_sq);
   }
 }
+
+TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) {
+  const auto t = TrajectoryGenerator::GenerateTrajectory(
+      std::vector<Pose2d>{Pose2d(0_m, 0_m, Rotation2d(0_deg)),
+                          Pose2d(1_m, 0_m, Rotation2d(180_deg))},
+      TrajectoryConfig(12_fps, 12_fps_sq));
+
+  ASSERT_EQ(t.States().size(), 1u);
+  ASSERT_EQ(t.TotalTime(), 0_s);
+}

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

@@ -294,7 +294,7 @@
   },
   {
     "name": "'Traditional' Hatchbot",
-    "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API.  Written in the 'traditional' style, i.e. commands are given their own classes.",
+    "description": "A fully-functional command-based hatchbot for the 2019 game using the new command framework.  Written in the 'traditional' style, i.e. commands are given their own classes.",
     "tags": [
       "Complete robot",
       "Command-based"
@@ -305,7 +305,7 @@
   },
   {
     "name": "'Inlined' Hatchbot",
-    "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API.  Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
+    "description": "A fully-functional command-based hatchbot for the 2019 game using the new command framework.  Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
     "tags": [
       "Complete robot",
       "Command-based",
@@ -317,7 +317,7 @@
   },
   {
     "name": "Select Command Example",
-    "description": "An example showing how to use the SelectCommand class from the experimental command framework rewrite.",
+    "description": "An example showing how to use the SelectCommand class from the new command framework.",
     "tags": [
       "Command-based"
     ],
@@ -327,7 +327,7 @@
   },
   {
     "name": "Scheduler Event Logging",
-    "description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the experimental command framework rewrite",
+    "description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the new command framework",
     "tags": [
       "Command-based",
       "Shuffleboard"

wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.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.                                                               */
@@ -14,6 +14,8 @@ import edu.wpi.first.hal.PWMJNI;
 
 /**
  * A class for driving addressable LEDs, such as WS2812s and NeoPixels.
+ *
+ * <p>Only 1 LED driver is currently supported by the roboRIO.
  */
 public class AddressableLED implements AutoCloseable {
   private final int m_pwmHandle;
@@ -22,7 +24,7 @@ public class AddressableLED implements AutoCloseable {
   /**
    * Constructs a new driver for a specific port.
    *
-   * @param port the output port to use (Must be a PWM port)
+   * @param port the output port to use (Must be a PWM header, not on MXP)
    */
   public AddressableLED(int port) {
     m_pwmHandle = PWMJNI.initializePWMPort(HAL.getPort((byte) port));
@@ -45,6 +47,8 @@ public class AddressableLED implements AutoCloseable {
    *
    * <p>Calling this is an expensive call, so its best to call it once, then just update data.
    *
+   * <p>The max length is 5460 LEDs.
+   *
    * @param length the strip length
    */
   public void setLength(int length) {

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

@@ -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.                                                               */
@@ -41,6 +41,25 @@ public class XboxController extends GenericHID {
     }
   }
 
+  /**
+   * Represents an axis on an XboxController.
+   */
+  public enum Axis {
+    kLeftX(0),
+    kRightX(4),
+    kLeftY(1),
+    kRightY(5),
+    kLeftTrigger(2),
+    kRightTrigger(3);
+
+    @SuppressWarnings({"MemberName", "PMD.SingularField"})
+    public final int value;
+
+    Axis(int value) {
+      this.value = value;
+    }
+  }
+
   /**
    * Construct an instance of a joystick. The joystick index is the USB port on the drivers
    * station.
@@ -62,9 +81,9 @@ public class XboxController extends GenericHID {
   @Override
   public double getX(Hand hand) {
     if (hand.equals(Hand.kLeft)) {
-      return getRawAxis(0);
+      return getRawAxis(Axis.kLeftX.value);
     } else {
-      return getRawAxis(4);
+      return getRawAxis(Axis.kRightX.value);
     }
   }
 
@@ -77,9 +96,9 @@ public class XboxController extends GenericHID {
   @Override
   public double getY(Hand hand) {
     if (hand.equals(Hand.kLeft)) {
-      return getRawAxis(1);
+      return getRawAxis(Axis.kLeftY.value);
     } else {
-      return getRawAxis(5);
+      return getRawAxis(Axis.kRightY.value);
     }
   }
 
@@ -91,9 +110,9 @@ public class XboxController extends GenericHID {
    */
   public double getTriggerAxis(Hand hand) {
     if (hand.equals(Hand.kLeft)) {
-      return getRawAxis(2);
+      return getRawAxis(Axis.kLeftTrigger.value);
     } else {
-      return getRawAxis(3);
+      return getRawAxis(Axis.kRightTrigger.value);
     }
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.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.                                                               */
@@ -328,7 +328,7 @@ public class PIDController implements Sendable, AutoCloseable {
   }
 
   /**
-   * Resets the previous error and the integral term. Also disables the controller.
+   * Resets the previous error and the integral term.
    */
   public void reset() {
     m_prevError = 0;

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.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.                                                               */
@@ -15,7 +15,8 @@ import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
 
 /**
  * Implements a PID control loop whose setpoint is constrained by a trapezoid
- * profile.
+ * profile. Users should call reset() when they first start running the controller
+ * to avoid unwanted behavior.
  */
 @SuppressWarnings("PMD.TooManyMethods")
 public class ProfiledPIDController implements Sendable {
@@ -319,10 +320,33 @@ public class ProfiledPIDController implements Sendable {
   }
 
   /**
-   * Reset the previous error, the integral term, and disable the controller.
+   * Reset the previous error and the integral term.
+   *
+   * @param measurement The current measured State of the system.
    */
-  public void reset() {
+  public void reset(TrapezoidProfile.State measurement) {
     m_controller.reset();
+    m_setpoint = measurement;
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system.
+   * @param measuredVelocity The current measured velocity of the system.
+   */
+  public void reset(double measuredPosition, double measuredVelocity) {
+    reset(new TrapezoidProfile.State(measuredPosition, measuredVelocity));
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system. The velocity is
+   *     assumed to be zero.
+   */
+  public void reset(double measuredPosition) {
+    reset(measuredPosition, 0.0);
   }
 
   @Override

wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.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.                                                               */
@@ -33,6 +33,9 @@ public class LiveWindow {
   private static boolean liveWindowEnabled;
   private static boolean telemetryEnabled = true;
 
+  private static Runnable enabledListener;
+  private static Runnable disabledListener;
+
   private static Component getOrAdd(Sendable sendable) {
     Component data = (Component) SendableRegistry.getData(sendable, dataHandle);
     if (data == null) {
@@ -46,6 +49,14 @@ public class LiveWindow {
     throw new UnsupportedOperationException("This is a utility class!");
   }
 
+  public static synchronized void setEnabledListener(Runnable runnable) {
+    enabledListener = runnable;
+  }
+
+  public static synchronized void setDisabledListener(Runnable runnable) {
+    disabledListener = runnable;
+  }
+
   public static synchronized boolean isEnabled() {
     return liveWindowEnabled;
   }
@@ -65,11 +76,17 @@ public class LiveWindow {
       updateValues(); // Force table generation now to make sure everything is defined
       if (enabled) {
         System.out.println("Starting live window mode.");
+        if (enabledListener != null) {
+          enabledListener.run();
+        }
       } else {
         System.out.println("stopping live window mode.");
         SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
           cbdata.builder.stopLiveWindowMode();
         });
+        if (disabledListener != null) {
+          disabledListener.run();
+        }
       }
       enabledEntry.setBoolean(enabled);
     }

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

@@ -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.                                                               */
@@ -55,7 +55,7 @@ public class SendableBuilderImpl implements SendableBuilder {
 
   private final List<Property> m_properties = new ArrayList<>();
   private Runnable m_safeState;
-  private Runnable m_updateTable;
+  private final List<Runnable> m_updateTables = new ArrayList<>();
   private NetworkTable m_table;
   private NetworkTableEntry m_controllableEntry;
   private boolean m_actuator;
@@ -105,8 +105,8 @@ public class SendableBuilderImpl implements SendableBuilder {
         property.m_update.accept(property.m_entry);
       }
     }
-    if (m_updateTable != null) {
-      m_updateTable.run();
+    for (Runnable updateTable : m_updateTables) {
+      updateTable.run();
     }
   }
 
@@ -207,7 +207,7 @@ public class SendableBuilderImpl implements SendableBuilder {
    */
   @Override
   public void setUpdateTable(Runnable func) {
-    m_updateTable = func;
+    m_updateTables.add(func);
   }
 
   /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.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.                                                               */
@@ -31,6 +31,7 @@
 
 package edu.wpi.first.wpilibj.spline;
 
+import java.util.ArrayDeque;
 import java.util.ArrayList;
 import java.util.List;
 
@@ -42,6 +43,36 @@ public final class SplineParameterizer {
   private static final double kMaxDy = 0.00127;
   private static final double kMaxDtheta = 0.0872;
 
+  /**
+   * A malformed spline does not actually explode the LIFO stack size. Instead, the stack size
+   * stays at a relatively small number (e.g. 30) and never decreases. Because of this, we must
+   * count iterations. Even long, complex paths don't usually go over 300 iterations, so hitting
+   * this maximum should definitely indicate something has gone wrong.
+   */
+  private static final int kMaxIterations = 5000;
+
+  @SuppressWarnings("MemberName")
+  private static class StackContents {
+    final double t1;
+    final double t0;
+
+    StackContents(double t0, double t1) {
+      this.t0 = t0;
+      this.t1 = t1;
+    }
+  }
+
+  public static class MalformedSplineException extends RuntimeException {
+    /**
+     * Create a new exception with the given message.
+     *
+     * @param message the message to pass with the exception
+     */
+    private MalformedSplineException(String message) {
+      super(message);
+    }
+  }
+
   /**
    * Private constructor because this is a utility class.
    */
@@ -53,7 +84,9 @@ public final class SplineParameterizer {
    * arcs until their dx, dy, and dtheta are within specific tolerances.
    *
    * @param spline The spline to parameterize.
-   * @return A vector of poses and curvatures that represents various points on the spline.
+   * @return A list of poses and curvatures that represents various points on the spline.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *                                  with approximately opposing headings)
    */
   public static List<PoseWithCurvature> parameterize(Spline spline) {
     return parameterize(spline, 0.0, 1.0);
@@ -66,41 +99,54 @@ public final class SplineParameterizer {
    * @param spline The spline to parameterize.
    * @param t0     Starting internal spline parameter. It is recommended to use 0.0.
    * @param t1     Ending internal spline parameter. It is recommended to use 1.0.
-   * @return A vector of poses and curvatures that represents various points on the spline.
+   * @return       A list of poses and curvatures that represents various points on the spline.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *                                  with approximately opposing headings)
    */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
   public static List<PoseWithCurvature> parameterize(Spline spline, double t0, double t1) {
-    var arr = new ArrayList<PoseWithCurvature>();
+    var splinePoints = new ArrayList<PoseWithCurvature>();
 
-    // The parameterization does not add the first initial point. Let's add
-    // that.
-    arr.add(spline.getPoint(t0));
+    // The parameterization does not add the initial point. Let's add that.
+    splinePoints.add(spline.getPoint(t0));
 
-    getSegmentArc(spline, arr, t0, t1);
-    return arr;
-  }
+    // We use an "explicit stack" to simulate recursion, instead of a recursive function call
+    // This give us greater control, instead of a stack overflow
+    var stack = new ArrayDeque<StackContents>();
+    stack.push(new StackContents(t0, t1));
 
-  /**
-   * Breaks up the spline into arcs until the dx, dy, and theta of each arc is
-   * within tolerance.
-   *
-   * @param spline The spline to parameterize.
-   * @param vector Pointer to vector of poses.
-   * @param t0     Starting point for arc.
-   * @param t1     Ending point for arc.
-   */
-  private static void getSegmentArc(Spline spline, List<PoseWithCurvature> vector,
-                                    double t0, double t1) {
-    final var start = spline.getPoint(t0);
-    final var end = spline.getPoint(t1);
+    StackContents current;
+    PoseWithCurvature start;
+    PoseWithCurvature end;
+    int iterations = 0;
 
-    final var twist = start.poseMeters.log(end.poseMeters);
+    while (!stack.isEmpty()) {
+      current = stack.removeFirst();
+      start = spline.getPoint(current.t0);
+      end = spline.getPoint(current.t1);
 
-    if (Math.abs(twist.dy) > kMaxDy || Math.abs(twist.dx) > kMaxDx
-        || Math.abs(twist.dtheta) > kMaxDtheta) {
-      getSegmentArc(spline, vector, t0, (t0 + t1) / 2);
-      getSegmentArc(spline, vector, (t0 + t1) / 2, t1);
-    } else {
-      vector.add(spline.getPoint(t1));
+      final var twist = start.poseMeters.log(end.poseMeters);
+      if (
+          Math.abs(twist.dy) > kMaxDy
+          || Math.abs(twist.dx) > kMaxDx
+          || Math.abs(twist.dtheta) > kMaxDtheta
+      ) {
+        stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1));
+        stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2));
+      } else {
+        splinePoints.add(spline.getPoint(current.t1));
+      }
+
+      iterations++;
+      if (iterations >= kMaxIterations) {
+        throw new MalformedSplineException(
+          "Could not parameterize a malformed spline. "
+          + "This means that you probably had two or more adjacent waypoints that were very close "
+          + "together with headings in opposing directions."
+        );
+      }
     }
+
+    return splinePoints;
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.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.                                                               */
@@ -8,9 +8,12 @@
 package edu.wpi.first.wpilibj.trajectory;
 
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.Collection;
 import java.util.List;
+import java.util.function.BiConsumer;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Transform2d;
@@ -19,14 +22,36 @@ import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
 import edu.wpi.first.wpilibj.spline.Spline;
 import edu.wpi.first.wpilibj.spline.SplineHelper;
 import edu.wpi.first.wpilibj.spline.SplineParameterizer;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
 
 public final class TrajectoryGenerator {
+  private static final Trajectory kDoNothingTrajectory =
+      new Trajectory(Arrays.asList(new Trajectory.State()));
+  private static BiConsumer<String, StackTraceElement[]> errorFunc;
+
   /**
    * Private constructor because this is a utility class.
    */
   private TrajectoryGenerator() {
   }
 
+  private static void reportError(String error, StackTraceElement[] stackTrace) {
+    if (errorFunc != null) {
+      errorFunc.accept(error, stackTrace);
+    } else {
+      DriverStation.reportError(error, stackTrace);
+    }
+  }
+
+  /**
+   * Set error reporting function. By default, DriverStation.reportError() is used.
+   *
+   * @param func Error reporting function, arguments are error and stackTrace.
+   */
+  public static void setErrorHandler(BiConsumer<String, StackTraceElement[]> func) {
+    errorFunc = func;
+  }
+
   /**
    * Generates a trajectory from the given control vectors and config. This method uses clamped
    * cubic splines -- a method in which the exterior control vectors and interior waypoints
@@ -60,9 +85,14 @@ public final class TrajectoryGenerator {
     }
 
     // Get the spline points
-    var points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(
-        newInitial, interiorWaypoints.toArray(new Translation2d[0]), newEnd
-    ));
+    List<PoseWithCurvature> points;
+    try {
+      points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(newInitial,
+          interiorWaypoints.toArray(new Translation2d[0]), newEnd));
+    } catch (MalformedSplineException ex) {
+      reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
 
     // Change the points back to their original orientation.
     if (config.isReversed()) {
@@ -130,9 +160,15 @@ public final class TrajectoryGenerator {
     }
 
     // Get the spline points
-    var points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors(
-        newControlVectors.toArray(new Spline.ControlVector[]{})
-    ));
+    List<PoseWithCurvature> points;
+    try {
+      points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors(
+          newControlVectors.toArray(new Spline.ControlVector[]{})
+      ));
+    } catch (MalformedSplineException ex) {
+      reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
 
     // Change the points back to their original orientation.
     if (config.isReversed()) {
@@ -171,6 +207,8 @@ public final class TrajectoryGenerator {
    *
    * @param splines The splines to parameterize.
    * @return The spline points for use in time parameterization of a trajectory.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *                                  with approximately opposing headings)
    */
   public static List<PoseWithCurvature> splinePointsFromSplines(
       Spline[] splines) {

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.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.                                                               */
@@ -47,6 +47,7 @@ public final class TrajectoryUtil {
    * @throws IOException if writing to the file fails
    */
   public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOException {
+    Files.createDirectories(path.getParent());
     try (BufferedWriter writer = Files.newBufferedWriter(path)) {
       WRITER.writeValue(writer, trajectory.getStates().toArray(new Trajectory.State[0]));
     }

wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.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.                                                               */
@@ -18,6 +18,61 @@ import edu.wpi.first.wpiutil.math.MathUtil;
  */
 @SuppressWarnings("MemberName")
 public class Color {
+  private static final double kPrecision = Math.pow(2, -12);
+
+  public final double red;
+  public final double green;
+  public final double blue;
+
+  /**
+   * Constructs a Color.
+   *
+   * @param red   Red value (0-1)
+   * @param green Green value (0-1)
+   * @param blue  Blue value (0-1)
+   */
+  public Color(double red, double green, double blue) {
+    this.red = roundAndClamp(red);
+    this.green = roundAndClamp(green);
+    this.blue = roundAndClamp(blue);
+  }
+
+  /**
+   * Constructs a Color from a Color8Bit.
+   *
+   * @param color The color
+   */
+  public Color(Color8Bit color) {
+    this(color.red / 255.0,
+        color.green / 255.0,
+        color.blue / 255.0);
+  }
+
+  @Override
+  public boolean equals(Object other) {
+    if (this == other) {
+      return true;
+    }
+    if (other == null || getClass() != other.getClass()) {
+      return false;
+    }
+
+    Color color = (Color) other;
+    return Double.compare(color.red, red) == 0
+        && Double.compare(color.green, green) == 0
+        && Double.compare(color.blue, blue) == 0;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(red, green, blue);
+  }
+
+  private static double roundAndClamp(double value) {
+    final var rounded = Math.round(value / kPrecision) * kPrecision;
+    return MathUtil.clamp(rounded, 0.0, 1.0);
+  }
+
   /*
    * FIRST Colors
    */
@@ -399,7 +454,7 @@ public class Color {
   /**
    * #20B2AA.
    */
-  public static final Color kLightSeagGeen = new Color(0.1254902f, 0.69803923f, 0.6666667f);
+  public static final Color kLightSeaGreen = new Color(0.1254902f, 0.69803923f, 0.6666667f);
 
   /**
    * #87CEFA.
@@ -414,7 +469,7 @@ public class Color {
   /**
    * #B0C4DE.
    */
-  public static final Color kLightSteellue = new Color(0.6901961f, 0.76862746f, 0.87058824f);
+  public static final Color kLightSteelBlue = new Color(0.6901961f, 0.76862746f, 0.87058824f);
 
   /**
    * #FFFFE0.
@@ -740,59 +795,4 @@ public class Color {
    * #9ACD32.
    */
   public static final Color kYellowGreen = new Color(0.6039216f, 0.8039216f, 0.19607843f);
-
-  public final double red;
-  public final double green;
-  public final double blue;
-
-  private static final double kPrecision = Math.pow(2, -12);
-
-  /**
-   * Constructs a Color.
-   *
-   * @param red   Red value (0-1)
-   * @param green Green value (0-1)
-   * @param blue  Blue value (0-1)
-   */
-  Color(double red, double green, double blue) {
-    this.red = roundAndClamp(red);
-    this.green = roundAndClamp(green);
-    this.blue = roundAndClamp(blue);
-  }
-
-  /**
-   * Constructs a Color from a Color8Bit.
-   *
-   * @param color The color
-   */
-  public Color(Color8Bit color) {
-    this(color.red / 255.0,
-        color.green / 255.0,
-        color.blue / 255.0);
-  }
-
-  @Override
-  public boolean equals(Object other) {
-    if (this == other) {
-      return true;
-    }
-    if (other == null || getClass() != other.getClass()) {
-      return false;
-    }
-
-    Color color = (Color) other;
-    return Double.compare(color.red, red) == 0
-        && Double.compare(color.green, green) == 0
-        && Double.compare(color.blue, blue) == 0;
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(red, green, blue);
-  }
-
-  private static double roundAndClamp(double value) {
-    final var rounded = Math.round(value / kPrecision) * kPrecision;
-    return MathUtil.clamp(rounded, 0.0, 1.0);
-  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class ProfiledPIDControllerTest {
+  @Test
+  void testStartFromNonZeroPosition() {
+    ProfiledPIDController controller = new ProfiledPIDController(1.0, 0.0, 0.0,
+        new TrapezoidProfile.Constraints(1.0, 1.0));
+
+    controller.reset(20);
+
+    assertEquals(0.0, controller.calculate(20), 0.05);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.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.                                                               */
@@ -8,6 +8,7 @@
 package edu.wpi.first.wpilibj.spline;
 
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 import org.junit.jupiter.api.Test;
@@ -15,9 +16,11 @@ import org.junit.jupiter.api.Test;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
 
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 
@@ -145,4 +148,15 @@ class CubicHermiteSplineTest {
 
     run(start, waypoints, end);
   }
+
+  @Test
+  void testMalformed() {
+    assertThrows(MalformedSplineException.class, () -> run(
+        new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+        new ArrayList<>(), new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+    assertThrows(MalformedSplineException.class, () -> run(
+        new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+        Arrays.asList(new Translation2d(10, 10.5)),
+        new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.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.                                                               */
@@ -13,9 +13,11 @@ import org.junit.jupiter.api.Test;
 
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
 
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 class QuinticHermiteSplineTest {
@@ -23,7 +25,7 @@ class QuinticHermiteSplineTest {
   private static final double kMaxDy = 0.00127;
   private static final double kMaxDtheta = 0.0872;
 
-  @SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"})
+  @SuppressWarnings({ "ParameterName", "PMD.UnusedLocalVariable" })
   private void run(Pose2d a, Pose2d b) {
     // Start the timer.
     var start = System.nanoTime();
@@ -49,29 +51,27 @@ class QuinticHermiteSplineTest {
       assertAll(
           () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
           () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
-          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta)
-      );
+          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta));
     }
 
     // Check first point
     assertAll(
-        () -> assertEquals(a.getTranslation().getX(),
-            poses.get(0).poseMeters.getTranslation().getX(), 1E-9),
-        () -> assertEquals(a.getTranslation().getY(),
-            poses.get(0).poseMeters.getTranslation().getY(), 1E-9),
-        () -> assertEquals(a.getRotation().getRadians(),
-            poses.get(0).poseMeters.getRotation().getRadians(), 1E-9)
-    );
+        () -> assertEquals(
+            a.getTranslation().getX(), poses.get(0).poseMeters.getTranslation().getX(), 1E-9),
+        () -> assertEquals(
+            a.getTranslation().getY(), poses.get(0).poseMeters.getTranslation().getY(), 1E-9),
+        () -> assertEquals(
+            a.getRotation().getRadians(), poses.get(0).poseMeters.getRotation().getRadians(),
+            1E-9));
 
     // Check last point
     assertAll(
-        () -> assertEquals(b.getTranslation().getX(),
-            poses.get(poses.size() - 1).poseMeters.getTranslation().getX(), 1E-9),
-        () -> assertEquals(b.getTranslation().getY(),
-            poses.get(poses.size() - 1).poseMeters.getTranslation().getY(), 1E-9),
+        () -> assertEquals(b.getTranslation().getX(), poses.get(poses.size() - 1)
+            .poseMeters.getTranslation().getX(), 1E-9),
+        () -> assertEquals(b.getTranslation().getY(), poses.get(poses.size() - 1)
+            .poseMeters.getTranslation().getY(), 1E-9),
         () -> assertEquals(b.getRotation().getRadians(),
-            poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9)
-    );
+            poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9));
   }
 
   @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@@ -89,7 +89,20 @@ class QuinticHermiteSplineTest {
   @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
   @Test
   void testSquiggly() {
-    run(new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
+    run(
+        new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
         new Pose2d(-1, 0, Rotation2d.fromDegrees(90)));
   }
+
+  @Test
+  void testMalformed() {
+    assertThrows(MalformedSplineException.class,
+        () -> run(
+          new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+          new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+    assertThrows(MalformedSplineException.class,
+        () -> run(
+          new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+          new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.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.                                                               */
@@ -8,10 +8,12 @@
 package edu.wpi.first.wpilibj.trajectory;
 
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 import org.junit.jupiter.api.Test;
 
+import edu.wpi.first.hal.sim.DriverStationSim;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Transform2d;
@@ -20,6 +22,7 @@ import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
 
 import static edu.wpi.first.wpilibj.util.Units.feetToMeters;
 import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 class TrajectoryGeneratorTest {
@@ -69,4 +72,24 @@ class TrajectoryGeneratorTest {
       );
     }
   }
+
+  @Test
+  void testMalformedTrajectory() {
+    var dsSim = new DriverStationSim();
+    dsSim.setSendError(false);
+
+    var traj =
+        TrajectoryGenerator.generateTrajectory(
+          Arrays.asList(
+            new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+            new Pose2d(1, 0, Rotation2d.fromDegrees(180))
+          ),
+          new TrajectoryConfig(feetToMeters(12), feetToMeters(12))
+        );
+
+    assertEquals(traj.getStates().size(), 1);
+    assertEquals(traj.getTotalTimeSeconds(), 0);
+
+    dsSim.setSendError(true);
+  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+import java.util.stream.Stream;
+
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+class ColorTest {
+  private static final double kEpsilon = 1e-3;
+
+  void assertColorMatches(double red, double green, double blue, Color color) {
+    assertAll(
+        () -> assertEquals(red, color.red, kEpsilon),
+        () -> assertEquals(green, color.green, kEpsilon),
+        () -> assertEquals(blue, color.blue, kEpsilon)
+    );
+  }
+
+  @ParameterizedTest
+  @MethodSource("staticColorProvider")
+  void staticColorTest(double red, double green, double blue, Color color) {
+    assertColorMatches(red, green, blue, color);
+  }
+
+  @ParameterizedTest
+  @MethodSource("staticColorProvider")
+  void colorEqualsTest(double red, double green, double blue, Color color) {
+    assertEquals(color, new Color(red, green, blue));
+  }
+
+  static Stream<Arguments> staticColorProvider() {
+    return Stream.of(
+      arguments(0.0823529412, 0.376470589, 0.7411764706, Color.kDenim),
+      arguments(0.0, 0.4, 0.7019607844, Color.kFirstBlue),
+      arguments(0.9294117648, 0.1098039216, 0.1411764706, Color.kFirstRed)
+    );
+  }
+}

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

@@ -40,7 +40,7 @@
     "commandversion": 1
   },
   {
-    "name": "Subsystem",
+    "name": "Subsystem (Old)",
     "description": "A subsystem",
     "tags": [
       "subsystem"

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

@@ -309,7 +309,7 @@
   },
   {
     "name": "'Traditional' Hatchbot",
-    "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API.  Written in the 'traditional' style, i.e. commands are given their own classes.",
+    "description": "A fully-functional command-based hatchbot for the 2019 game using the new command framework.  Written in the 'traditional' style, i.e. commands are given their own classes.",
     "tags": [
       "Complete robot",
       "Command-based"
@@ -321,7 +321,7 @@
   },
   {
     "name": "'Inlined' Hatchbot",
-    "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API.  Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
+    "description": "A fully-functional command-based hatchbot for the 2019 game using the new command framework.  Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
     "tags": [
       "Complete robot",
       "Command-based",
@@ -334,7 +334,7 @@
   },
   {
     "name": "Select Command Example",
-    "description": "An example showing how to use the SelectCommand class from the experimental command framework rewrite.",
+    "description": "An example showing how to use the SelectCommand class from the new command framework.",
     "tags": [
       "Command-based"
     ],
@@ -345,7 +345,7 @@
   },
   {
     "name": "Scheduler Event Logging",
-    "description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the experimental command framework rewrite",
+    "description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the new command framework",
     "tags": [
       "Command-based",
       "Shuffleboard"

wpiutil/src/main/native/include/units/units.h

@@ -2042,7 +2042,7 @@ namespace units
 		{
 			return detail::abs(nls::m_value - units::convert<UnitsRhs, Units>(rhs.m_value)) < std::numeric_limits<T>::epsilon() * 
 				detail::abs(nls::m_value + units::convert<UnitsRhs, Units>(rhs.m_value)) ||
-				detail::abs(nls::m_value - units::convert<UnitsRhs, Units>(rhs.m_value)) < std::numeric_limits<T>::min();
+				detail::abs(nls::m_value - units::convert<UnitsRhs, Units>(rhs.m_value)) < (std::numeric_limits<T>::min)();
 		}
 
 		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs, std::enable_if_t<std::is_integral<T>::value && std::is_integral<Ty>::value, int> = 0>
@@ -2656,14 +2656,14 @@ namespace units
 	constexpr bool operator==(const UNIT_LIB_DEFAULT_TYPE lhs, const Units& rhs) noexcept
 	{
 		return detail::abs(lhs - static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs)) < std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::epsilon() * detail::abs(lhs + static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs)) ||
-			detail::abs(lhs - static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs)) < std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::min();
+			detail::abs(lhs - static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs)) < (std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::min)();
 	}
 
 	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
 	constexpr bool operator==(const Units& lhs, const UNIT_LIB_DEFAULT_TYPE rhs) noexcept
 	{
 		return detail::abs(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) - rhs) < std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::epsilon() * detail::abs(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) + rhs) ||
-			detail::abs(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) - rhs) < std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::min();
+			detail::abs(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) - rhs) < (std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::min)();
 	}
 
 	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
@@ -4166,7 +4166,7 @@ namespace units
 		//----------------------------------
 
 		template<class UnitTypeLhs, class UnitTypeRhs>
-		UnitTypeLhs min(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs)
+		UnitTypeLhs (min)(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs)
 		{
 			static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value, "Unit types are not compatible.");
 			UnitTypeLhs r(rhs);
@@ -4174,7 +4174,7 @@ namespace units
 		}
 
 		template<class UnitTypeLhs, class UnitTypeRhs>
-		UnitTypeLhs max(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs)
+		UnitTypeLhs (max)(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs)
 		{
 			static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value, "Unit types are not compatible.");
 			UnitTypeLhs r(rhs);
@@ -4806,13 +4806,13 @@ namespace std
 	class numeric_limits<units::unit_t<Units, T, NonLinearScale>>
 	{
 	public:
-		static constexpr units::unit_t<Units, T, NonLinearScale> min()
+		static constexpr units::unit_t<Units, T, NonLinearScale> (min)()
 		{
-			return units::unit_t<Units, T, NonLinearScale>(std::numeric_limits<T>::min());
+			return units::unit_t<Units, T, NonLinearScale>((std::numeric_limits<T>::min)());
 		}
-		static constexpr units::unit_t<Units, T, NonLinearScale> max()
+		static constexpr units::unit_t<Units, T, NonLinearScale> (max)()
 		{
-			return units::unit_t<Units, T, NonLinearScale>(std::numeric_limits<T>::max());
+			return units::unit_t<Units, T, NonLinearScale>((std::numeric_limits<T>::max)());
 		}
 		static constexpr units::unit_t<Units, T, NonLinearScale> lowest()
 		{

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

@@ -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.                                                               */
@@ -10,6 +10,8 @@
 
 #include <string>
 
+#include "wpi/Twine.h"
+
 namespace wpi {
 
 /**
@@ -18,7 +20,7 @@ namespace wpi {
  * @param mangledSymbol the mangled symbol.
  * @return The demangled symbol, or mangledSymbol if demangling fails.
  */
-std::string Demangle(char const* mangledSymbol);
+std::string Demangle(const Twine& mangledSymbol);
 
 }  // namespace wpi
 

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

@@ -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.                                                               */
@@ -11,26 +11,26 @@
 
 #include <cstdio>
 
+#include "wpi/SmallString.h"
+
 namespace wpi {
 
-std::string Demangle(char const* mangledSymbol) {
-  char buffer[256];
+std::string Demangle(const Twine& mangledSymbol) {
+  SmallString<128> buf;
   size_t length;
   int32_t status;
 
-  if (std::sscanf(mangledSymbol, "%*[^(]%*[(]%255[^)+]", buffer)) {
-    char* symbol = abi::__cxa_demangle(buffer, nullptr, &length, &status);
-    if (status == 0) {
-      return symbol;
-    } else {
-      // If the symbol couldn't be demangled, it's probably a C function,
-      // so just return it as-is.
-      return buffer;
-    }
+  char* symbol =
+      abi::__cxa_demangle(mangledSymbol.toNullTerminatedStringRef(buf).data(),
+                          nullptr, &length, &status);
+  if (status == 0) {
+    std::string rv{symbol};
+    std::free(symbol);
+    return rv;
   }
 
   // If everything else failed, just return the mangled symbol
-  return mangledSymbol;
+  return mangledSymbol.str();
 }
 
 }  // namespace wpi

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

@@ -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.                                                               */
@@ -11,6 +11,7 @@
 
 #include "wpi/Demangle.h"
 #include "wpi/SmallString.h"
+#include "wpi/StringRef.h"
 #include "wpi/raw_ostream.h"
 
 namespace wpi {
@@ -25,7 +26,11 @@ std::string GetStackTrace(int offset) {
   for (int i = offset; i < stackSize; i++) {
     // Only print recursive functions once in a row.
     if (i == 0 || stackTrace[i] != stackTrace[i - 1]) {
-      trace << "\tat " << Demangle(mangledSymbols[i]) << "\n";
+      // extract just function name from "pathToExe(functionName+offset)"
+      StringRef sym{mangledSymbols[i]};
+      sym = sym.split('(').second;
+      sym = sym.split('+').first;
+      trace << "\tat " << Demangle(sym) << "\n";
     }
   }