Add ArrayRef overloads to new command classes (#2216)

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

.wpilib/wpilib_preferences.json

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

CONTRIBUTING.md

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

FasterBuilds.md

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

azure-pipelines-testbench.yaml

@@ -1,19 +1,91 @@
-# Starter pipeline
-# Start with a minimal pipeline that you can customize to build and deploy your code.
-# Add steps that build, run tests, deploy, and more:
-# https://aka.ms/yaml
-
-trigger:
-- master
-
-pool:
-  vmImage: 'ubuntu-latest'
-
-steps:
-- script: echo Hello, world!
-  displayName: 'Run a one-line script'
-
-- script: |
-    echo Add other tasks to build, test, and deploy your project.
-    echo See https://aka.ms/yaml
-  displayName: 'Run a multi-line script'
+# Testing steps for real hardware
+
+trigger:
+  batch: true
+  branches:
+    include:
+    - master
+
+stages:
+- stage: Build
+  jobs:
+  - job: IntegrationTests
+    displayName: Integration Tests
+    pool:
+      vmImage: 'Ubuntu 16.04'
+
+    container:
+      image:  wpilib/roborio-cross-ubuntu:2020-18.04
+
+    timeoutInMinutes: 0
+
+    steps:
+    - task: Gradle@2
+      condition: and(succeeded(), not(startsWith(variables['Build.SourceBranch'], 'refs/tags/v')))
+      inputs:
+        workingDirectory: ''
+        gradleWrapperFile: 'gradlew'
+        gradleOptions: '-Xmx3072m'
+        publishJUnitResults: false
+        testResultsFiles: '**/TEST-*.xml'
+        tasks: 'copyWpilibJIntegrationTestJarToOutput copyWpilibCTestLibrariesToOutput'
+        options: '-Ponlylinuxathena -PbuildServer'
+
+    - task: PublishPipelineArtifact@0
+      inputs:
+        artifactName: 'Integration Tests'
+        targetPath: 'build/integrationTestFiles'
+
+- stage: TestBench
+  displayName: Test Bench
+  jobs:
+  - job: Cpp
+    displayName: C++
+    pool: RoboRioConnections
+    timeoutInMinutes: 30
+    workspace:
+      clean: all
+    steps:
+    - task: DownloadPipelineArtifact@0
+      inputs:
+        artifactName: 'Integration Tests'
+        targetPath: 'build/integrationTestFiles'
+
+    - task: ShellScript@2
+      displayName: Run C++ Tests
+      inputs:
+        scriptPath: test-scripts/deploy-and-run-test-on-robot.sh
+        args: 'cpp -A "--gtest_output=xml:/home/admin/testResults/cppreport.xml"'
+
+    - task: PublishTestResults@2
+      displayName: Publish C++ Test Results
+      inputs:
+        testResultsFormat: 'JUnit'
+        testResultsFiles: '*.xml'
+        testRunTitle: 'C++ Test Report'
+        searchFolder: '$(System.DefaultWorkingDirectory)/test-reports'
+
+  - job: Java
+    pool: RoboRioConnections
+    timeoutInMinutes: 30
+    workspace:
+      clean: all
+    steps:
+    - task: DownloadPipelineArtifact@0
+      inputs:
+        artifactName: 'Integration Tests'
+        targetPath: 'build/integrationTestFiles'
+
+    - task: ShellScript@2
+      displayName: Run Java Tests
+      inputs:
+        scriptPath: test-scripts/deploy-and-run-test-on-robot.sh
+        args: 'java'
+
+    - task: PublishTestResults@2
+      displayName: Publish Java Test Results
+      inputs:
+        testResultsFormat: 'JUnit'
+        testResultsFiles: '*.xml'
+        testRunTitle: 'Java Test Report'
+        searchFolder: '$(System.DefaultWorkingDirectory)/test-reports'

build.gradle

@@ -6,7 +6,7 @@ plugins {
     id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2020.2'
     id 'edu.wpi.first.NativeUtils' apply false
     id 'edu.wpi.first.GradleJni' version '0.10.1'
-    id 'edu.wpi.first.GradleVsCode' version '0.10.0'
+    id 'edu.wpi.first.GradleVsCode' version '0.11.0'
     id 'idea'
     id 'visual-studio'
     id 'net.ltgt.errorprone' version '1.1.1' apply false

buildSrc/build.gradle

@@ -5,5 +5,5 @@ repositories {
     }
 }
 dependencies {
-    implementation "edu.wpi.first:native-utils:2020.5.2"
+    implementation "edu.wpi.first:native-utils:2020.7.2"
 }

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

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

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

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

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

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

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

@@ -1197,7 +1197,7 @@ void UsbCameraImpl::SetQuirks() {
   if (deviceNum >= 0) {
     int vendorId, productId;
     if (GetVendorProduct(deviceNum, &vendorId, &productId)) {
-      m_ps3eyecam_exposure = vendorId == 0x2000 && productId == 0x0145;
+      m_ps3eyecam_exposure = vendorId == 0x1415 && productId == 0x2000;
     }
   }
 }

docs/build.gradle

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

hal/build.gradle

@@ -192,7 +192,7 @@ model {
             if (!(it instanceof NativeBinarySpec)) return
             if (it.component.name != 'hal' && it.component.name != 'halBase') return
             if (it.targetPlatform.name != nativeUtils.wpi.platforms.roborio) return
-            nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared')
+            nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared')
         }
     }
 }

hal/src/generate/Instances.txt

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

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

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

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

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

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

@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.sim;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.hal.SimValue;
+import edu.wpi.first.hal.sim.mockdata.SimDeviceDataJNI;
+
+public class SimDeviceSim {
+  private final int m_handle;
+
+  public SimDeviceSim(String name) {
+    m_handle = SimDeviceDataJNI.getSimDeviceHandle(name);
+  }
+
+  public SimValue getValue(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimValue(handle);
+  }
+
+  public SimDouble getDouble(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimDouble(handle);
+  }
+
+  public SimEnum getEnum(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimEnum(handle);
+  }
+
+  public SimBoolean getBoolean(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimBoolean(handle);
+  }
+
+  public static String[] getEnumOptions(SimEnum val) {
+    return SimDeviceDataJNI.getSimValueEnumOptions(val.getNativeHandle());
+  }
+
+  public SimDeviceDataJNI.SimValueInfo[] enumerateValues() {
+    return SimDeviceDataJNI.enumerateSimValues(m_handle);
+  }
+
+  public int getNativeHandle() {
+    return m_handle;
+  }
+
+  public CallbackStore registerValueCreatedCallback(SimValueCallback callback, boolean initialNotify) {
+    int uid = SimDeviceDataJNI.registerSimValueCreatedCallback(m_handle, callback, initialNotify);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueCreatedCallback);
+  }
+
+  public CallbackStore registerValueChangedCallback(SimValueCallback callback, boolean initialNotify) {
+    int uid = SimDeviceDataJNI.registerSimValueChangedCallback(m_handle, callback, initialNotify);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueChangedCallback);
+  }
+
+  public static SimDeviceDataJNI.SimDeviceInfo[] enumerateDevices(String prefix) {
+    return SimDeviceDataJNI.enumerateSimDevices(prefix);
+  }
+
+  public CallbackStore registerDeviceCreatedCallback(String prefix, SimDeviceCallback callback, boolean initialNotify) {
+    int uid = SimDeviceDataJNI.registerSimDeviceCreatedCallback(prefix, callback, initialNotify);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimDeviceCreatedCallback);
+  }
+
+  public CallbackStore registerDeviceFreedCallback(String prefix, SimDeviceCallback callback) {
+    int uid = SimDeviceDataJNI.registerSimDeviceFreedCallback(prefix, callback);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimDeviceFreedCallback);
+  }
+
+  public static void resetData() {
+    SimDeviceDataJNI.resetSimDeviceData();
+  }
+}

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

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

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

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

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

@@ -224,26 +224,38 @@ double HAL_GetAnalogAverageVoltage(HAL_AnalogInputHandle analogPortHandle,
 
 int32_t HAL_GetAnalogLSBWeight(HAL_AnalogInputHandle analogPortHandle,
                                int32_t* status) {
-  auto port = analogInputHandles->Get(analogPortHandle);
-  if (port == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return 0;
-  }
-  int32_t lsbWeight = FRC_NetworkCommunication_nAICalibration_getLSBWeight(
-      0, port->channel, status);  // XXX: aiSystemIndex == 0?
-  return lsbWeight;
+  // On the roboRIO, LSB is the same for all channels. So the channel lookup can
+  // be avoided
+  return FRC_NetworkCommunication_nAICalibration_getLSBWeight(0, 0, status);
+
+  // Keep the old code for future hardware
+
+  // auto port = analogInputHandles->Get(analogPortHandle);
+  // if (port == nullptr) {
+  //   *status = HAL_HANDLE_ERROR;
+  //   return 0;
+  // }
+  // int32_t lsbWeight = FRC_NetworkCommunication_nAICalibration_getLSBWeight(
+  //     0, port->channel, status);  // XXX: aiSystemIndex == 0?
+  // return lsbWeight;
 }
 
 int32_t HAL_GetAnalogOffset(HAL_AnalogInputHandle analogPortHandle,
                             int32_t* status) {
-  auto port = analogInputHandles->Get(analogPortHandle);
-  if (port == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return 0;
-  }
-  int32_t offset = FRC_NetworkCommunication_nAICalibration_getOffset(
-      0, port->channel, status);  // XXX: aiSystemIndex == 0?
-  return offset;
+  // On the roboRIO, offset is the same for all channels. So the channel lookup
+  // can be avoided
+  return FRC_NetworkCommunication_nAICalibration_getOffset(0, 0, status);
+
+  // Keep the old code for future hardware
+
+  // auto port = analogInputHandles->Get(analogPortHandle);
+  // if (port == nullptr) {
+  //   *status = HAL_HANDLE_ERROR;
+  //   return 0;
+  // }
+  // int32_t offset = FRC_NetworkCommunication_nAICalibration_getOffset(
+  //     0, port->channel, status);  // XXX: aiSystemIndex == 0?
+  // return offset;
 }
 
 }  // extern "C"

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

@@ -11,6 +11,7 @@
 #include <cstddef>
 #include <cstring>
 #include <memory>
+#include <type_traits>
 
 #include "AnalogInternal.h"
 #include "DigitalInternal.h"
@@ -28,7 +29,8 @@
 
 using namespace hal;
 
-static_assert(std::is_pod_v<HAL_DMASample>, "DMA Sample must be POD");
+static_assert(std::is_standard_layout_v<HAL_DMASample>,
+              "HAL_DMASample must have standard layout");
 
 namespace {
 

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

@@ -118,7 +118,11 @@ void* HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle,
   if (anInterrupt == nullptr) {
     return nullptr;
   }
-  anInterrupt->manager->disable(status);
+
+  if (anInterrupt->manager->isEnabled(status)) {
+    anInterrupt->manager->disable(status);
+  }
+
   void* param = anInterrupt->param;
   return param;
 }
@@ -152,7 +156,10 @@ void HAL_EnableInterrupts(HAL_InterruptHandle interruptHandle,
     *status = HAL_HANDLE_ERROR;
     return;
   }
-  anInterrupt->manager->enable(status);
+
+  if (!anInterrupt->manager->isEnabled(status)) {
+    anInterrupt->manager->enable(status);
+  }
 }
 
 void HAL_DisableInterrupts(HAL_InterruptHandle interruptHandle,
@@ -162,7 +169,9 @@ void HAL_DisableInterrupts(HAL_InterruptHandle interruptHandle,
     *status = HAL_HANDLE_ERROR;
     return;
   }
-  anInterrupt->manager->disable(status);
+  if (anInterrupt->manager->isEnabled(status)) {
+    anInterrupt->manager->disable(status);
+  }
 }
 
 int64_t HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,

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

@@ -175,7 +175,11 @@ double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) {
   HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
                            &receivedTimestamp, TimeoutMs, status);
 
-  return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966;
+  if (*status != 0) {
+    return 0;
+  } else {
+    return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966;
+  }
 }
 
 double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
@@ -186,7 +190,11 @@ double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
   HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
                            &receivedTimestamp, TimeoutMs, status);
 
-  return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
+  if (*status != 0) {
+    return 0;
+  } else {
+    return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
+  }
 }
 
 double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
@@ -205,6 +213,9 @@ double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
     PdpStatus1 pdpStatus;
     HAL_ReadCANPacketTimeout(handle, Status1, pdpStatus.data, &length,
                              &receivedTimestamp, TimeoutMs, status);
+    if (*status != 0) {
+      return 0;
+    }
     switch (channel) {
       case 0:
         raw = (static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
@@ -235,6 +246,9 @@ double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
     PdpStatus2 pdpStatus;
     HAL_ReadCANPacketTimeout(handle, Status2, pdpStatus.data, &length,
                              &receivedTimestamp, TimeoutMs, status);
+    if (*status != 0) {
+      return 0;
+    }
     switch (channel) {
       case 6:
         raw = (static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
@@ -265,6 +279,9 @@ double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
     PdpStatus3 pdpStatus;
     HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
                              &receivedTimestamp, TimeoutMs, status);
+    if (*status != 0) {
+      return 0;
+    }
     switch (channel) {
       case 12:
         raw = (static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
@@ -365,6 +382,9 @@ double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
 
   HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
                            &receivedTimestamp, TimeoutMs, status);
+  if (*status != 0) {
+    return 0;
+  }
 
   uint32_t raw;
   raw = pdpStatus.bits.TotalCurrent_125mAperunit_h8;
@@ -380,6 +400,9 @@ double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status) {
 
   HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
                            &receivedTimestamp, TimeoutMs, status);
+  if (*status != 0) {
+    return 0;
+  }
 
   uint32_t raw;
   raw = pdpStatus.bits.Power_125mWperunit_h4;
@@ -397,6 +420,9 @@ double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
 
   HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
                            &receivedTimestamp, TimeoutMs, status);
+  if (*status != 0) {
+    return 0;
+  }
 
   uint32_t raw;
   raw = pdpStatus.bits.Energy_125mWPerUnitXTmeas_h4;

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

@@ -565,7 +565,7 @@ void HAL_StopSPIAuto(HAL_SPIPort port, int32_t* status) {
 void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend,
                                 int32_t dataSize, int32_t zeroSize,
                                 int32_t* status) {
-  if (dataSize < 0 || dataSize > 16) {
+  if (dataSize < 0 || dataSize > 32) {
     *status = PARAMETER_OUT_OF_RANGE;
     return;
   }
@@ -589,7 +589,7 @@ void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend,
   // set byte counts
   tSPI::tAutoByteCount config;
   config.ZeroByteCount = static_cast<unsigned>(zeroSize) & 0x7f;
-  config.TxByteCount = static_cast<unsigned>(dataSize) & 0xf;
+  config.TxByteCount = static_cast<unsigned>(dataSize) & 0x1f;
   spiSystem->writeAutoByteCount(config, status);
 }
 
@@ -631,4 +631,21 @@ int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) {
   return spiSystem->readTransferSkippedFullCount(status);
 }
 
+void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
+                               int32_t stallTicks, int32_t pow2BytesPerRead,
+                               int32_t* status) {
+  std::scoped_lock lock(spiAutoMutex);
+  // FPGA only has one auto SPI engine
+  if (port != spiAutoPort) {
+    *status = INCOMPATIBLE_STATE;
+    return;
+  }
+
+  tSPI::tStallConfig stallConfig;
+  stallConfig.CsToSclkTicks = static_cast<uint8_t>(csToSclkTicks);
+  stallConfig.StallTicks = static_cast<uint16_t>(stallTicks);
+  stallConfig.Pow2BytesPerRead = static_cast<uint8_t>(pow2BytesPerRead);
+  spiSystem->writeStallConfig(stallConfig, status);
+}
+
 }  // extern "C"

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

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

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

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

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include "hal/Types.h"
+
 /**
  * @defgroup hal_extensions Simulator Extensions
  * @ingroup hal_capi
@@ -41,5 +43,19 @@ int HAL_LoadOneExtension(const char* library);
  * @return        the succes state of the initialization
  */
 int HAL_LoadExtensions(void);
+
+/**
+ * Enables or disables the message saying no HAL extensions were found.
+ *
+ * Some apps don't care, and the message create clutter. For general team code,
+ * we want it.
+ *
+ * This must be called before HAL_Initialize is called.
+ *
+ * This defaults to true.
+ *
+ * @param showMessage true to show message, false to not.
+ */
+void HAL_SetShowExtensionsNotFoundMessages(HAL_Bool showMessage);
 }  // extern "C"
 /** @} */

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

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

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

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

hal/src/main/native/include/simulation/SimDeviceSim.h

@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "CallbackStore.h"
+#include "hal/SimDevice.h"
+#include "mockdata/SimDeviceData.h"
+
+namespace frc {
+namespace sim {
+class SimDeviceSim {
+ public:
+  explicit SimDeviceSim(const char* name)
+      : m_handle{HALSIM_GetSimDeviceHandle(name)} {}
+
+  hal::SimValue GetValue(const char* name) const {
+    return HALSIM_GetSimValueHandle(m_handle, name);
+  }
+
+  hal::SimDouble GetDouble(const char* name) const {
+    return HALSIM_GetSimValueHandle(m_handle, name);
+  }
+
+  hal::SimEnum GetEnum(const char* name) const {
+    return HALSIM_GetSimValueHandle(m_handle, name);
+  }
+
+  hal::SimBoolean GetBoolean(const char* name) const {
+    return HALSIM_GetSimValueHandle(m_handle, name);
+  }
+
+  static std::vector<std::string> GetEnumOptions(hal::SimEnum val) {
+    int32_t numOptions;
+    const char** options = HALSIM_GetSimValueEnumOptions(val, &numOptions);
+    std::vector<std::string> rv;
+    rv.reserve(numOptions);
+    for (int32_t i = 0; i < numOptions; ++i) rv.emplace_back(options[i]);
+    return rv;
+  }
+
+  template <typename F>
+  void EnumerateValues(F callback) const {
+    return HALSIM_EnumerateSimValues(
+        m_handle, &callback,
+        [](const char* name, void* param, HAL_SimValueHandle handle,
+           HAL_Bool readonly, const struct HAL_Value* value) {
+          std::invoke(*static_cast<F*>(param), name, handle, readonly, value);
+        });
+  }
+
+  operator HAL_SimDeviceHandle() const { return m_handle; }
+
+  template <typename F>
+  static void EnumerateDevices(const char* prefix, F callback) {
+    return HALSIM_EnumerateSimDevices(
+        prefix, &callback,
+        [](const char* name, void* param, HAL_SimDeviceHandle handle) {
+          std::invoke(*static_cast<F*>(param), name, handle);
+        });
+  }
+
+  static void ResetData() { HALSIM_ResetSimDeviceData(); }
+
+ private:
+  HAL_SimDeviceHandle m_handle;
+};
+}  // namespace sim
+}  // namespace frc

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -26,8 +26,16 @@ void HAL_CAN_SendMessage(uint32_t messageID, const uint8_t* data,
 void HAL_CAN_ReceiveMessage(uint32_t* messageID, uint32_t messageIDMask,
                             uint8_t* data, uint8_t* dataSize,
                             uint32_t* timeStamp, int32_t* status) {
+  // Use a data size of 42 as call check. Difficult to add check to invoke
+  // handler
+  *dataSize = 42;
+  auto tmpStatus = *status;
   SimCanData->receiveMessage(messageID, messageIDMask, data, dataSize,
                              timeStamp, status);
+  // If no handler invoked, return message not found
+  if (*dataSize == 42 && *status == tmpStatus) {
+    *status = HAL_ERR_CANSessionMux_MessageNotFound;
+  }
 }
 void HAL_CAN_OpenStreamSession(uint32_t* sessionHandle, uint32_t messageID,
                                uint32_t messageIDMask, uint32_t maxMessages,

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

@@ -41,6 +41,11 @@ void InitializeExtensions() {}
 }  // namespace init
 }  // namespace hal
 
+static bool& GetShowNotFoundMessage() {
+  static bool showMsg = true;
+  return showMsg;
+}
+
 extern "C" {
 
 int HAL_LoadOneExtension(const char* library) {
@@ -91,8 +96,10 @@ int HAL_LoadExtensions(void) {
   wpi::SmallVector<wpi::StringRef, 2> libraries;
   const char* e = std::getenv("HALSIM_EXTENSIONS");
   if (!e) {
-    wpi::outs() << "HAL Extensions: No extensions found\n";
-    wpi::outs().flush();
+    if (GetShowNotFoundMessage()) {
+      wpi::outs() << "HAL Extensions: No extensions found\n";
+      wpi::outs().flush();
+    }
     return rc;
   }
   wpi::StringRef env{e};
@@ -105,4 +112,8 @@ int HAL_LoadExtensions(void) {
   return rc;
 }
 
+void HAL_SetShowExtensionsNotFoundMessages(HAL_Bool showMessage) {
+  GetShowNotFoundMessage() = showMessage;
+}
+
 }  // extern "C"

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

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

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

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

hal/src/test/java/edu/wpi/first/hal/sim/SimDeviceSimTest.java

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.sim;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class SimDeviceSimTest {
+  @Test
+  void testBasic() {
+    SimDevice dev = SimDevice.create("test");
+    SimBoolean devBool = dev.createBoolean("bool", false, false);
+
+    SimDeviceSim sim = new SimDeviceSim("test");
+    SimBoolean simBool = sim.getBoolean("bool");
+
+    assertFalse(simBool.get());
+    simBool.set(true);
+    assertTrue(devBool.get());
+  }
+}

hal/src/test/native/cpp/sim/SimDeviceSimTest.cpp

@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <wpi/StringRef.h>
+
+#include "gtest/gtest.h"
+#include "hal/SimDevice.h"
+#include "simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+namespace hal {
+
+TEST(SimDeviceSimTests, TestBasic) {
+  SimDevice dev{"test"};
+  SimBoolean devBool = dev.CreateBoolean("bool", false, false);
+
+  SimDeviceSim sim{"test"};
+  SimBoolean simBool = sim.GetBoolean("bool");
+  EXPECT_FALSE(simBool.Get());
+  simBool.Set(true);
+  EXPECT_TRUE(devBool.Get());
+}
+
+TEST(SimDeviceSimTests, TestEnumerateDevices) {
+  SimDevice dev{"test"};
+
+  bool foundit = false;
+  SimDeviceSim::EnumerateDevices(
+      "te", [&](const char* name, HAL_SimDeviceHandle handle) {
+        if (wpi::StringRef(name) == "test") foundit = true;
+      });
+  EXPECT_TRUE(foundit);
+}
+
+}  // namespace hal

myRobot/build.gradle

@@ -86,18 +86,27 @@ deploy {
 }
 
 tasks.register('deployJava') {
-    dependsOn tasks.named('deployJreRoborio')
-    dependsOn tasks.named('deployMyRobotJavaRoborio')
-    dependsOn tasks.named('deployMyRobotCppLibrariesRoborio')
+    try {
+        dependsOn tasks.named('deployJreRoborio')
+        dependsOn tasks.named('deployMyRobotJavaRoborio')
+        dependsOn tasks.named('deployMyRobotCppLibrariesRoborio')
+    } catch (ignored) {
+    }
 }
 
 tasks.register('deployShared') {
-    dependsOn tasks.named('deployMyRobotCppLibrariesRoborio')
-    dependsOn tasks.named('deployMyRobotCppRoborio')
+    try {
+        dependsOn tasks.named('deployMyRobotCppLibrariesRoborio')
+        dependsOn tasks.named('deployMyRobotCppRoborio')
+    } catch (ignored) {
+    }
 }
 
 tasks.register('deployStatic') {
-    dependsOn tasks.named('deployMyRobotCppStaticRoborio')
+    try {
+        dependsOn tasks.named('deployMyRobotCppStaticRoborio')
+    } catch (ignored) {
+    }
 }
 
 mainClassName = 'Main'
@@ -148,7 +157,7 @@ model {
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                     lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                     if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                        nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'ni_runtime_shared')
+                        nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
                     }
             }
         }
@@ -177,7 +186,7 @@ model {
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
                     lib project: ':cameraserver', library: 'cameraserver', linkage: 'static'
                     if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                        nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'ni_runtime_shared')
+                        nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
                     }
             }
         }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

shared/config.gradle

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

shared/cppDesktopTestTask.gradle

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

shared/examplecheck.gradle

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

shared/javaDesktopTestTask.gradle

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

shared/javacpp/setupBuild.gradle

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

shared/jni/setupBuild.gradle

@@ -200,7 +200,7 @@ model {
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                 }
                 if (nativeName == 'hal' && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                    nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'ni_runtime_shared')
+                    nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
                 }
             }
         }
@@ -235,7 +235,7 @@ model {
             if (!project.hasProperty('noWpiutil')) {
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                 if (nativeName == 'hal' && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                    nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'ni_runtime_shared')
+                    nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
                 }
             }
         }
@@ -275,6 +275,9 @@ model {
     }
 }
 
+apply from: "${rootDir}/shared/cppDesktopTestTask.gradle"
+apply from: "${rootDir}/shared/javaDesktopTestTask.gradle"
+
 ext.getJniSpecClass = {
     return JniNativeLibrarySpec
 }

shared/plugins/setupBuild.gradle

@@ -57,7 +57,7 @@ if (!project.hasProperty('onlylinuxathena')) {
                         }
                         lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                         if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                            nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'ni_runtime_shared')
+                            nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
                         }
                     } else {
                         it.buildable = false

simulation/halsim_ds_socket/build.gradle

@@ -49,7 +49,7 @@ model {
             lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
             lib library: pluginName, linkage: 'shared'
             if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'ni_runtime_shared')
+                nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
             }
         }
     }

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

@@ -23,8 +23,14 @@
 
 using namespace halsimgui;
 
-static constexpr int kDefaultColumns = 10;
-static std::vector<int> numColumns;
+namespace {
+struct LEDDisplaySettings {
+  int numColumns = 10;
+  LEDConfig config;
+};
+}  // namespace
+
+static std::vector<LEDDisplaySettings> displaySettings;
 
 // read/write columns setting to ini file
 static void* AddressableLEDReadOpen(ImGuiContext* ctx,
@@ -33,15 +39,15 @@ static void* AddressableLEDReadOpen(ImGuiContext* ctx,
   int num;
   if (wpi::StringRef{name}.getAsInteger(10, num)) return nullptr;
   if (num < 0) return nullptr;
-  if (num >= static_cast<int>(numColumns.size()))
-    numColumns.resize(num + 1, kDefaultColumns);
-  return &numColumns[num];
+  if (num >= static_cast<int>(displaySettings.size()))
+    displaySettings.resize(num + 1);
+  return &displaySettings[num];
 }
 
 static void AddressableLEDReadLine(ImGuiContext* ctx,
                                    ImGuiSettingsHandler* handler, void* entry,
                                    const char* lineStr) {
-  int* cols = static_cast<int*>(entry);
+  auto* settings = static_cast<LEDDisplaySettings*>(entry);
   // format: columns=#
   wpi::StringRef line{lineStr};
   auto [name, value] = line.split('=');
@@ -50,24 +56,41 @@ static void AddressableLEDReadLine(ImGuiContext* ctx,
   if (name == "columns") {
     int num;
     if (value.getAsInteger(10, num)) return;
-    *cols = num;
+    settings->numColumns = num;
+  } else if (name == "serpentine") {
+    int num;
+    if (value.getAsInteger(10, num)) return;
+    settings->config.serpentine = num != 0;
+  } else if (name == "order") {
+    int num;
+    if (value.getAsInteger(10, num)) return;
+    settings->config.order = static_cast<LEDConfig::Order>(num);
+  } else if (name == "start") {
+    int num;
+    if (value.getAsInteger(10, num)) return;
+    settings->config.start = static_cast<LEDConfig::Start>(num);
   }
 }
 
 static void AddressableLEDWriteAll(ImGuiContext* ctx,
                                    ImGuiSettingsHandler* handler,
                                    ImGuiTextBuffer* out_buf) {
-  for (size_t i = 0; i < numColumns.size(); ++i) {
-    out_buf->appendf("[AddressableLED][%d]\ncolumns=%d\n\n",
-                     static_cast<int>(i), numColumns[i]);
+  for (size_t i = 0; i < displaySettings.size(); ++i) {
+    out_buf->appendf(
+        "[AddressableLED][%d]\ncolumns=%d\nserpentine=%d\norder=%d\n"
+        "start=%d\n\n",
+        static_cast<int>(i), displaySettings[i].numColumns,
+        displaySettings[i].config.serpentine ? 1 : 0,
+        static_cast<int>(displaySettings[i].config.order),
+        static_cast<int>(displaySettings[i].config.start));
   }
 }
 
 static void DisplayAddressableLEDs() {
   bool hasAny = false;
   static const int numLED = HAL_GetNumAddressableLEDs();
-  if (numLED > static_cast<int>(numColumns.size()))
-    numColumns.resize(numLED, kDefaultColumns);
+  if (numLED > static_cast<int>(displaySettings.size()))
+    displaySettings.resize(numLED);
 
   for (int i = 0; i < numLED; ++i) {
     if (!HALSIM_GetAddressableLEDInitialized(i)) continue;
@@ -82,8 +105,22 @@ static void DisplayAddressableLEDs() {
     ImGui::PushItemWidth(ImGui::GetFontSize() * 6);
     ImGui::LabelText("Length", "%d", length);
     ImGui::LabelText("Running", "%s", running ? "Yes" : "No");
-    ImGui::InputInt("Columns", &numColumns[i]);
-    if (numColumns[i] < 1) numColumns[i] = 1;
+    ImGui::InputInt("Columns", &displaySettings[i].numColumns);
+    {
+      static const char* options[] = {"Row Major", "Column Major"};
+      int val = displaySettings[i].config.order;
+      if (ImGui::Combo("Order", &val, options, 2))
+        displaySettings[i].config.order = static_cast<LEDConfig::Order>(val);
+    }
+    {
+      static const char* options[] = {"Upper Left", "Lower Left", "Upper Right",
+                                      "Lower Right"};
+      int val = displaySettings[i].config.start;
+      if (ImGui::Combo("Start", &val, options, 4))
+        displaySettings[i].config.start = static_cast<LEDConfig::Start>(val);
+    }
+    ImGui::Checkbox("Serpentine", &displaySettings[i].config.serpentine);
+    if (displaySettings[i].numColumns < 1) displaySettings[i].numColumns = 1;
     ImGui::PopItemWidth();
 
     // show as LED indicators
@@ -100,7 +137,8 @@ static void DisplayAddressableLEDs() {
       }
     }
 
-    DrawLEDs(values, length, numColumns[i], colors);
+    DrawLEDs(values, length, displaySettings[i].numColumns, colors, 0, 0,
+             displaySettings[i].config);
   }
   if (!hasAny) ImGui::Text("No addressable LEDs");
 }

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

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

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

@@ -10,30 +10,87 @@
 namespace halsimgui {
 
 void DrawLEDs(const int* values, int numValues, int cols, const ImU32* colors,
-              float size, float spacing) {
-  if (numValues == 0) return;
+              float size, float spacing, const LEDConfig& config) {
+  if (numValues == 0 || cols < 1) return;
   if (size == 0) size = ImGui::GetFontSize() / 2.0;
   if (spacing == 0) spacing = ImGui::GetFontSize() / 3.0;
 
+  int rows = (numValues + cols - 1) / cols;
+  float inc = size + spacing;
+
   ImDrawList* drawList = ImGui::GetWindowDrawList();
   const ImVec2 p = ImGui::GetCursorScreenPos();
-  float x = p.x + size / 2, y = p.y + size / 2;
-  int rows = 1;
-  for (int i = 0; i < numValues; ++i) {
-    if (i >= (rows * cols)) {
-      ++rows;
-      x = p.x + size / 2;
-      y += size + spacing;
-    }
-    if (values[i] > 0)
-      drawList->AddRectFilled(ImVec2(x, y), ImVec2(x + size, y + size),
-                              colors[values[i] - 1]);
-    else if (values[i] < 0)
-      drawList->AddRect(ImVec2(x, y), ImVec2(x + size, y + size),
-                        colors[-values[i] - 1], 0.0f, 0, 1.0);
-    x += size + spacing;
+
+  float ystart, yinc;
+  if (config.start & 1) {
+    // lower
+    ystart = p.y + size / 2 + inc * (rows - 1);
+    yinc = -inc;
+  } else {
+    // upper
+    ystart = p.y + size / 2;
+    yinc = inc;
   }
-  ImGui::Dummy(ImVec2((size + spacing) * cols, (size + spacing) * rows));
+
+  float xstart, xinc;
+  if (config.start & 2) {
+    // right
+    xstart = p.x + size / 2 + inc * (cols - 1);
+    xinc = -inc;
+  } else {
+    // left
+    xstart = p.x + size / 2;
+    xinc = inc;
+  }
+
+  float x = xstart, y = ystart;
+  if (config.order == LEDConfig::RowMajor) {
+    // row major
+    int row = 1;
+    for (int i = 0; i < numValues; ++i) {
+      if (i >= (row * cols)) {
+        ++row;
+        if (config.serpentine) {
+          x -= xinc;
+          xinc = -xinc;
+        } else {
+          x = xstart;
+        }
+        y += yinc;
+      }
+      if (values[i] > 0)
+        drawList->AddRectFilled(ImVec2(x, y), ImVec2(x + size, y + size),
+                                colors[values[i] - 1]);
+      else if (values[i] < 0)
+        drawList->AddRect(ImVec2(x, y), ImVec2(x + size, y + size),
+                          colors[-values[i] - 1], 0.0f, 0, 1.0);
+      x += xinc;
+    }
+  } else {
+    // column major
+    int col = 1;
+    for (int i = 0; i < numValues; ++i) {
+      if (i >= (col * rows)) {
+        ++col;
+        if (config.serpentine) {
+          y -= yinc;
+          yinc = -yinc;
+        } else {
+          y = ystart;
+        }
+        x += xinc;
+      }
+      if (values[i] > 0)
+        drawList->AddRectFilled(ImVec2(x, y), ImVec2(x + size, y + size),
+                                colors[values[i] - 1]);
+      else if (values[i] < 0)
+        drawList->AddRect(ImVec2(x, y), ImVec2(x + size, y + size),
+                          colors[-values[i] - 1], 0.0f, 0, 1.0);
+      y += yinc;
+    }
+  }
+
+  ImGui::Dummy(ImVec2(inc * cols, inc * rows));
 }
 
 }  // namespace halsimgui

simulation/halsim_gui/src/main/native/include/ExtraGuiWidgets.h

@@ -11,6 +11,33 @@
 
 namespace halsimgui {
 
+/**
+ * DrawLEDs() configuration for 2D arrays.
+ */
+struct LEDConfig {
+  /**
+   * Whether the major order is serpentined (e.g. the first row goes left to
+   * right, the second row right to left, the third row left to right, and so
+   * on).
+   */
+  bool serpentine = false;
+
+  /**
+   * The input array order (row-major or column-major).
+   */
+  enum Order { RowMajor = 0, ColumnMajor } order = RowMajor;
+
+  /**
+   * The starting location of the array (0 location).
+   */
+  enum Start {
+    UpperLeft = 0,
+    LowerLeft,
+    UpperRight,
+    LowerRight
+  } start = UpperLeft;
+};
+
 /**
  * Draw a 2D array of LEDs.
  *
@@ -27,8 +54,10 @@ namespace halsimgui {
  *             if 0, defaults to 1/2 of font size
  * @param spacing spacing between each LED (both horizontal and vertical);
  *                if 0, defaults to 1/3 of font size
+ * @param config 2D array configuration
  */
 void DrawLEDs(const int* values, int numValues, int cols, const ImU32* colors,
-              float size = 0.0f, float spacing = 0.0f);
+              float size = 0.0f, float spacing = 0.0f,
+              const LEDConfig& config = LEDConfig{});
 
 }  // namespace halsimgui

simulation/lowfi_simulation/build.gradle

@@ -116,7 +116,7 @@ if (!project.hasProperty('onlylinuxathena')) {
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                     lib library: nativeName, linkage: 'shared'
                     if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                        nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'ni_runtime_shared')
+                        nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
                     }
                 }
             }
@@ -131,7 +131,7 @@ if (!project.hasProperty('onlylinuxathena')) {
                 lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
                 lib library: nativeName, linkage: 'shared'
                 if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                    nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'ni_runtime_shared')
+                    nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
                 }
             }
         }

test-scripts/README.md

@@ -1,17 +1,19 @@
-# WPILIB TEST SCRIPTS
+# WPILib Test Scripts
+
 ## Overview
+
 These test scripts are designed to allow the user of the WPILib test framework to quickly and easily deploy and run their tests on the WPI roboRIO.
 
-In order for the automated test system to work there is a driverstation onboard the roboRIO that handles a queue of users waiting to use the driver station. All of the interaction with this queue is handled internally by test scripts contained within this folder.
-
-If you deploy code to the test stand using the Eclipse plugins, you _must_ remove the build artifacts in `/home/lvuser`, or you will break tests.
+If you deploy code to the test stand using GradleRIO, you _must_ remove the build artifacts in `/home/lvuser`, or you will break the test stand.
 
 ## roboRIO Setup
-The roboRIO on the test bench must be updated everytime NI releases a new image.
+The roboRIO on the test bench must be updated every time NI releases a new image.
 
-1. [Use the roboRIO Imaging Tool to format the roboRIO with the lastest image.](https://wpilib.screenstepslive.com/s/4485/m/13503/l/144984-imaging-your-roborio)
-2. [Install Java on the roboRIO.](https://wpilib.screenstepslive.com/s/4485/m/13503/l/599747-installing-java-8-on-the-roborio-using-the-frc-roborio-java-installer-java-only)
-3. SFTP the [teststand, netconsole, and libstdc++ ipk files](https://users.wpi.edu/~phplenefisch/ipk/) on to the roboRIO.
-4. ssh on to the roboRIO as the admin user (ex: `ssh admin@roboRIO-190-FRC.local`)
-5. Use opkg to install the ipk files (ex: `opkg install teststand_1.2-1_armv7a-vfp.ipk`)
-6. Reboot the roboRIO
+1. [Use the roboRIO Imaging Tool to format the roboRIO with the lastest image.](https://frcdocs.wpi.edu/en/latest/docs/getting-started/getting-started-frc-control-system/imaging-your-roborio.html)
+2. Set a static ip on the roboRIO web dashboard to `10.1.90.2`
+2. Install Java on the roboRIO
+    1. [Download the JRE from Maven.](https://frcmaven.wpi.edu/artifactory/list/release/edu/wpi/first/jdk/)
+    2. Transfer the JRE ipk to the roboRIO with scp: `scp <local path> admin@roboRIO-190-FRC.local:/tmp/frcjre.ipk`
+    3. Install the JRE: `opkg install /tmp/frcjre.ipk`
+    4. Remove the ipk file: `rm /tmp/frcjre.ipk`
+3. Reboot the roboRIO

test-scripts/config.sh

@@ -1,9 +1,9 @@
 #!/usr/bin/env bash
 #*----------------------------------------------------------------------------*#
-#* Copyright (c) FIRST 2014. All Rights Reserved.							  *#
+#* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        *#
 #* Open Source Software - may be modified and shared by FRC teams. The code   *#
 #* must be accompanied by the FIRST BSD license file in the root directory of *#
-#* the project.															      *#
+#* the project.                                                               *#
 #*----------------------------------------------------------------------------*#
 
 # If this is changed, update the .gitignore
@@ -31,7 +31,7 @@ DEFAULT_DESTINATION_CPP_TEST_RESULTS=${DEFAULT_DESTINATION_TEST_RESULTS_DIR}/${C
 DEFAULT_JAVA_TEST_NAME=FRCUserProgram.jar
 DEFAULT_JAVA_TEST_ARGS=""
 
-DEFAULT_LOCAL_JAVA_TEST_FILE=../build/integrationTestFiles/java/wpilibjIntegrationTests.jar
+DEFAULT_LOCAL_JAVA_TEST_FILE=../build/integrationTestFiles/java/wpilibjIntegrationTests-all.jar
 
 JAVA_REPORT=javareport.xml
 DEFAULT_LIBRARY_NATIVE_FILES=../build/integrationTestFiles/libs

test-scripts/deploy-and-run-test-on-robot.sh

@@ -1,9 +1,9 @@
 #!/usr/bin/env bash
 #*----------------------------------------------------------------------------*#
-#* Copyright (c) FIRST 2014. All Rights Reserved.							  *#
+#* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        *#
 #* Open Source Software - may be modified and shared by FRC teams. The code   *#
 #* must be accompanied by the FIRST BSD license file in the root directory of *#
-#* the project.															      *#
+#* the project.                                                               *#
 #*----------------------------------------------------------------------------*#
 
 # Configurable variables
@@ -19,7 +19,7 @@ DEFAULT_DESTINATION_RUN_TEST_SCRIPT=${DEFAULT_DESTINATION_DIR}/${DEFAULT_LOCAL_R
 
 usage="$(basename "$0") [-h] (java|cpp) [-A] [arg] [arg]...
 A script designed to run the integration tests.
-This script should only be run on the roborio.
+This script should only be run on the computer connected to the roboRIO.
 Where:
     -h    Show this help text.
     -A    Disable language recomended arguments.
@@ -30,20 +30,26 @@ Where:
 LANGUAGE=none
 LOCAL_TEST_FILE=none
 DESTINATION_TEST_FILE=none
-LIBRARY_FILES=none
 TEST_RUN_ARGS=""
+DESTINATION_TEST_RESULTS=none
+LOCAL_TEST_RESULTS=none
 
-# Begin searching for options from the third paramater on
+
+# Begin searching for options from the second paramater on
 PARAM_ARGS=${@:2}
 
 if [[ "$1" = java ]]; then
     LANGUAGE=$1
     LOCAL_TEST_FILE=$DEFAULT_LOCAL_JAVA_TEST_FILE
     DESTINATION_TEST_FILE=$DEFAULT_DESTINATION_JAVA_TEST_FILE
+    DESTINATION_TEST_RESULTS=$DEFAULT_DESTINATION_JAVA_TEST_RESULTS
+    LOCAL_TEST_RESULTS=$DEFAULT_LOCAL_JAVA_TEST_RESULT
 elif [[ "$1" = cpp ]]; then
     LANGUAGE=$1
     LOCAL_TEST_FILE=$DEFAULT_LOCAL_CPP_TEST_FILE
     DESTINATION_TEST_FILE=$DEFAULT_DESTINATION_CPP_TEST_FILE
+    DESTINATION_TEST_RESULTS=$DEFAULT_DESTINATION_CPP_TEST_RESULTS
+    LOCAL_TEST_RESULTS=$DEFAULT_LOCAL_CPP_TEST_RESULT
 elif [[ "$1" = "-h" ]]; then
     printf "Usage:\n"
     echo "$usage"
@@ -65,25 +71,14 @@ TEST_RUN_ARGS="${@:2}"
 
 shopt -s huponexit
 
-SCP_TEST_SCRIPT="scp config.sh ${DEFAULT_LOCAL_RUN_TEST_SCRIPT} ${ROBOT_ADDRESS}:/${DEFAULT_DESTINATION_DIR}"
-SSH_CHMOD_AND_MAKE_TEMP_TEST_DIR="ssh -t ${ROBOT_ADDRESS} \"chmod a+x ${DEFAULT_DESTINATION_RUN_TEST_SCRIPT}; mkdir ${DEFAULT_TEST_SCP_DIR}; touch ${DESTINATION_TEST_FILE}\""
-SCP_TEST_PROGRAM="scp ${LOCAL_TEST_FILE} ${ROBOT_ADDRESS}:${DESTINATION_TEST_FILE}"
-SSH_RUN_TESTS="ssh -t ${ROBOT_ADDRESS} ${DEFAULT_DESTINATION_RUN_TEST_SCRIPT} ${LANGUAGE} $(whoami) -d ${DEFAULT_TEST_SCP_DIR} ${TEST_RUN_ARGS}"
-SCP_NATIVE_LIBRARIES="scp ${DEFAULT_LIBRARY_NATIVE_FILES}/* ${ROBOT_ADDRESS}:${DEFAULT_LIBRARY_NATIVE_DESTINATION}"
-CONFIG_NATIVE_LIBRARIES="ssh -t ${ADMIN_ROBOT_ADDRESS} ldconfig"
+# Fail if any command fails
+set -e
 
-if [ $(which sshpass) ]; then
-    sshpass -p "" ${SCP_NATIVE_LIBRARIES}
-    sshpass -p "" ${CONFIG_NATIVE_LIBRARIES}
-    sshpass -p "" ${SCP_TEST_SCRIPT}
-    sshpass -p "" ${SSH_CHMOD_AND_MAKE_TEMP_TEST_DIR}
-    sshpass -p "" ${SCP_TEST_PROGRAM}
-    sshpass -p "" ${SSH_RUN_TESTS}
-else
-    eval ${SCP_NATIVE_LIBRARIES}
-    eval ${CONFIG_NATIVE_LIBRARIES}
-    eval ${SCP_TEST_SCRIPT}
-    eval ${SSH_CHMOD_AND_MAKE_TEMP_TEST_DIR}
-    eval ${SCP_TEST_PROGRAM}
-    eval ${SSH_RUN_TESTS}
-fi
+ssh ${ROBOT_ADDRESS} "rm -R ${DEFAULT_DESTINATION_TEST_RESULTS_DIR}; mkdir ${DEFAULT_DESTINATION_TEST_RESULTS_DIR}"
+scp ${DEFAULT_LIBRARY_NATIVE_FILES}/* ${ROBOT_ADDRESS}:${DEFAULT_LIBRARY_NATIVE_DESTINATION}
+ssh ${ADMIN_ROBOT_ADDRESS} ldconfig
+scp config.sh ${DEFAULT_LOCAL_RUN_TEST_SCRIPT} ${ROBOT_ADDRESS}:/${DEFAULT_DESTINATION_DIR}
+ssh ${ROBOT_ADDRESS} "chmod a+x ${DEFAULT_DESTINATION_RUN_TEST_SCRIPT}; mkdir ${DEFAULT_TEST_SCP_DIR}; touch ${DESTINATION_TEST_FILE}"
+scp ${LOCAL_TEST_FILE} ${ROBOT_ADDRESS}:${DESTINATION_TEST_FILE}
+ssh ${ROBOT_ADDRESS} ${DEFAULT_DESTINATION_RUN_TEST_SCRIPT} ${LANGUAGE} -d ${DEFAULT_TEST_SCP_DIR} ${TEST_RUN_ARGS}
+mkdir ${DEFAULT_LOCAL_TEST_RESULTS_DIR}; scp ${ROBOT_ADDRESS}:${DESTINATION_TEST_RESULTS} ${LOCAL_TEST_RESULTS}

test-scripts/jenkins-run-tests-get-results.sh

@@ -1,79 +0,0 @@
-#!/usr/bin/env bash
-#*----------------------------------------------------------------------------*#
-#* Copyright (c) FIRST 2014. 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.															      *#
-#*----------------------------------------------------------------------------*#
-
-# Configurable variables
-source config.sh
-
-(
-# Wait for lock
-printf "Getting exclusive lock for RIO execution...\n"
-flock -x 200 || exit 1
-
-# Ensure the teststand is dead
-SSH_STOP_TESTSTAND="ssh -t ${ROBOT_ADDRESS} sh -c '/etc/init.d/teststand stop; sleep 1'"
-if [ $(which sshpass) ]; then
-    sshpass -p "" ${SSH_STOP_TESTSTAND}
-else
-    eval ${SSH_STOP_TESTSTAND}
-fi
-
-# If there are already test results in the repository then remove them
-if [[ -e ${DEFAULT_LOCAL_TEST_RESULTS_DIR} ]]; then
-    rm -R ${DEFAULT_LOCAL_TEST_RESULTS_DIR}
-fi
-
-# Make the directory where the tests should live
-mkdir ${DEFAULT_LOCAL_TEST_RESULTS_DIR} 2>/dev/null
-
-# Remove the preivous test results from the the robot
-SSH_REMOVE_OLD_TEST_RESULTS="ssh -t ${ROBOT_ADDRESS} rm -R ${DEFAULT_DESTINATION_TEST_RESULTS_DIR}; mkdir ${DEFAULT_DESTINATION_TEST_RESULTS_DIR}"
-if [ $(which sshpass) ]; then
-    sshpass -p "" ${SSH_REMOVE_OLD_TEST_RESULTS}
-else
-    eval ${SSH_REMOVE_OLD_TEST_RESULTS}
-fi
-
-printf "Running cpp test\n"
-
-# Run the C++ Tests
-./deploy-and-run-test-on-robot.sh cpp -A "--gtest_output=xml:${DEFAULT_DESTINATION_CPP_TEST_RESULTS}"
-
-# Retrive the C++ Test Results
-SCP_GET_CPP_TEST_RESULT="scp ${ROBOT_ADDRESS}:${DEFAULT_DESTINATION_CPP_TEST_RESULTS} ${DEFAULT_LOCAL_CPP_TEST_RESULT}"
-if [ $(which sshpass) ]; then
-    sshpass -p "" ${SCP_GET_CPP_TEST_RESULT}
-else
-    eval ${SCP_GET_CPP_TEST_RESULT}
-fi
-
-sleep 10
-
-# Run the Java Tests
-./deploy-and-run-test-on-robot.sh java
-
-# Retrive the Java Test Results
-SCP_GET_JAVA_TEST_RESULT="scp ${ROBOT_ADDRESS}:${DEFAULT_DESTINATION_JAVA_TEST_RESULTS} ${DEFAULT_LOCAL_JAVA_TEST_RESULT}"
-if [ $(which sshpass) ]; then
-    sshpass -p "" ${SCP_GET_JAVA_TEST_RESULT}
-else
-    eval ${SCP_GET_JAVA_TEST_RESULT}
-fi
-
-# Make sure that we got test results back.
-if [ ! -e ${DEFAULT_LOCAL_CPP_TEST_RESULT} ]; then
-  echo "There are no results from the C++ tests; they must have failed."
-  exit 100
-fi
-
-if [ ! -e ${DEFAULT_LOCAL_JAVA_TEST_RESULT} ]; then
-  echo "There are no results from the Java tests; they must have failed."
-  exit 101
-fi
-
-# The mutex is released when this program exits
-) 200>/var/lock/jenkins.rio.exclusivelock

test-scripts/run-tests-on-robot.sh

@@ -1,9 +1,9 @@
 #!/usr/bin/env bash
 #*----------------------------------------------------------------------------*#
-#* Copyright (c) FIRST 2014. All Rights Reserved.							  *#
+#* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        *#
 #* Open Source Software - may be modified and shared by FRC teams. The code   *#
 #* must be accompanied by the FIRST BSD license file in the root directory of *#
-#* the project.															      *#
+#* the project.                                                               *#
 #*----------------------------------------------------------------------------*#
 
 # This file is intended to be run in the DEFAULT_TEST_DIR on the roboRIO.
@@ -17,12 +17,11 @@ source config.sh
 DEFAULT_TEST_DIR=${DEFAULT_DESTINATION_DIR}
 DEFAULT_PATH_TO_JRE=/usr/local/frc/JRE/bin/java
 
-usage="$(basename "$0") [-h] (java|cpp) name [-d test_dir] [-A] [arg] [arg]...
+usage="$(basename "$0") [-h] (java|cpp) [-d test_dir] [-A] [arg] [arg]...
 A script designed to run the integration tests.
 This script should only be run on the roborio.
 Where:
     -h    Show this help text
-    name  The name of the user trying to run the tests (used for driver station)
     -d    The directory where the tests have been placed.
           This is done to prevent overwriting an already running program.
           This scrip will automatically move the test into the ${DEFAULT_TEST_DIR}
@@ -42,12 +41,11 @@ fi
 
 LANGUAGE=none
 TEST_FILE=none
-NAME=$2
 TEST_DIR="$DEFAULT_TEST_DIR"
-# Begin searching for options from the third paramater on
-PARAM_ARGS=${@:3}
+# Begin searching for options from the second paramater on
+PARAM_ARGS=${@:2}
 # Where the test arguments start
-TEST_RUN_ARGS=${@:3}
+TEST_RUN_ARGS=${@:2}
 RUN_WITH_DEFAULT_ARGS=true
 DEFAULT_ARGS=""
 
@@ -70,7 +68,7 @@ else
     exit 1
 fi
 
-PARAM_COUNTER=2
+PARAM_COUNTER=1
 printf "Param Args ${PARAM_ARGS}\n"
 
 # Check for optional paramaters

wpilibNewCommands/build.gradle

@@ -64,7 +64,7 @@ model {
                 lib project: ':cscore', library: 'cscore', linkage: 'shared'
             }
             if ((it instanceof NativeExecutableBinarySpec || it instanceof GoogleTestTestSuiteBinarySpec) && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'ni_runtime_shared')
+                nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
             }
         }
     }

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

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

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

@@ -19,20 +19,32 @@ public abstract class PIDSubsystem extends SubsystemBase {
   protected final PIDController m_controller;
   protected boolean m_enabled;
 
+  private double m_setpoint;
+
   /**
    * Creates a new PIDSubsystem.
    *
    * @param controller the PIDController to use
+   * @param initialPosition the initial setpoint of the subsystem
+   */
+  public PIDSubsystem(PIDController controller, double initialPosition) {
+    setSetpoint(initialPosition);
+    m_controller = requireNonNullParam(controller, "controller", "PIDSubsystem");
+  }
+
+  /**
+   * Creates a new PIDSubsystem.  Initial setpoint is zero.
+   *
+   * @param controller the PIDController to use
    */
   public PIDSubsystem(PIDController controller) {
-    requireNonNullParam(controller, "controller", "PIDSubsystem");
-    m_controller = controller;
+    this(controller, 0);
   }
 
   @Override
   public void periodic() {
     if (m_enabled) {
-      useOutput(m_controller.calculate(getMeasurement(), getSetpoint()));
+      useOutput(m_controller.calculate(getMeasurement(), m_setpoint), m_setpoint);
     }
   }
 
@@ -40,26 +52,29 @@ public abstract class PIDSubsystem extends SubsystemBase {
     return m_controller;
   }
 
+  /**
+   * Sets the setpoint for the subsystem.
+   *
+   * @param setpoint the setpoint for the subsystem
+   */
+  public void setSetpoint(double setpoint) {
+    m_setpoint = setpoint;
+  }
+
   /**
    * Uses the output from the PIDController.
    *
    * @param output the output of the PIDController
+   * @param setpoint the setpoint of the PIDController (for feedforward)
    */
-  public abstract void useOutput(double output);
-
-  /**
-   * Returns the reference (setpoint) used by the PIDController.
-   *
-   * @return the reference (setpoint) to be used by the controller
-   */
-  public abstract double getSetpoint();
+  protected abstract void useOutput(double output, double setpoint);
 
   /**
    * Returns the measurement of the process variable used by the PIDController.
    *
    * @return the measurement of the process variable
    */
-  public abstract double getMeasurement();
+  protected abstract double getMeasurement();
 
   /**
    * Enables the PID control.  Resets the controller.
@@ -74,6 +89,15 @@ public abstract class PIDSubsystem extends SubsystemBase {
    */
   public void disable() {
     m_enabled = false;
-    useOutput(0);
+    useOutput(0, 0);
+  }
+
+  /**
+   * Returns whether the controller is enabled.
+   *
+   * @return Whether the controller is enabled.
+   */
+  public boolean isEnabled() {
+    return m_enabled;
   }
 }

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.                                                               */
@@ -8,6 +8,7 @@
 package edu.wpi.first.wpilibj2.command;
 
 import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
 
 import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
@@ -20,20 +21,33 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
   protected final ProfiledPIDController m_controller;
   protected boolean m_enabled;
 
+  private TrapezoidProfile.State m_goal;
+
   /**
    * Creates a new ProfiledPIDSubsystem.
    *
    * @param controller the ProfiledPIDController to use
+   * @param initialPosition the initial goal position of the controller
+   */
+  public ProfiledPIDSubsystem(ProfiledPIDController controller,
+                              double initialPosition) {
+    m_controller = requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
+    setGoal(initialPosition);
+  }
+
+  /**
+   * Creates a new ProfiledPIDSubsystem.  Initial goal position is zero.
+   *
+   * @param controller the ProfiledPIDController to use
    */
   public ProfiledPIDSubsystem(ProfiledPIDController controller) {
-    requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
-    m_controller = controller;
+    this(controller, 0);
   }
 
   @Override
   public void periodic() {
     if (m_enabled) {
-      useOutput(m_controller.calculate(getMeasurement(), getGoal()), m_controller.getSetpoint());
+      useOutput(m_controller.calculate(getMeasurement(), m_goal), m_controller.getSetpoint());
     }
   }
 
@@ -41,34 +55,45 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
     return m_controller;
   }
 
+  /**
+   * Sets the goal state for the subsystem.
+   *
+   * @param goal The goal state for the subsystem's motion profile.
+   */
+  public void setGoal(TrapezoidProfile.State goal) {
+    m_goal = goal;
+  }
+
+  /**
+   * Sets the goal state for the subsystem.  Goal velocity assumed to be zero.
+   *
+   * @param goal The goal position for the subsystem's motion profile.
+   */
+  public void setGoal(double goal) {
+    setGoal(new TrapezoidProfile.State(goal, 0));
+  }
+
   /**
    * Uses the output from the ProfiledPIDController.
    *
    * @param output   the output of the ProfiledPIDController
    * @param setpoint the setpoint state of the ProfiledPIDController, for feedforward
    */
-  public abstract void useOutput(double output, State setpoint);
-
-  /**
-   * Returns the goal used by the ProfiledPIDController.
-   *
-   * @return the goal to be used by the controller
-   */
-  public abstract State getGoal();
+  protected abstract void useOutput(double output, State setpoint);
 
   /**
    * Returns the measurement of the process variable used by the ProfiledPIDController.
    *
    * @return the measurement of the process variable
    */
-  public abstract double getMeasurement();
+  protected abstract double getMeasurement();
 
   /**
    * Enables the PID control.  Resets the controller.
    */
   public void enable() {
     m_enabled = true;
-    m_controller.reset();
+    m_controller.reset(getMeasurement());
   }
 
   /**
@@ -78,4 +103,13 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
     m_enabled = false;
     useOutput(0, new State());
   }
+
+  /**
+   * Returns whether the controller is enabled.
+   *
+   * @return Whether the controller is enabled.
+   */
+  public boolean isEnabled() {
+    return m_enabled;
+  }
 }

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

@@ -53,8 +53,8 @@ public class RamseteCommand extends CommandBase {
 
   /**
    * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
-   * PID control and feedforward are handled internally, and outputs are scaled -1 to 1 for easy
-   * consumption by speed controllers.
+   * 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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -9,23 +9,29 @@
 
 using namespace frc2;
 
-PIDSubsystem::PIDSubsystem(PIDController controller)
-    : m_controller{controller} {}
+PIDSubsystem::PIDSubsystem(PIDController controller, double initialPosition)
+    : m_controller{controller} {
+  SetSetpoint(initialPosition);
+}
 
 void PIDSubsystem::Periodic() {
   if (m_enabled) {
-    UseOutput(m_controller.Calculate(GetMeasurement(), GetSetpoint()));
+    UseOutput(m_controller.Calculate(GetMeasurement(), m_setpoint), m_setpoint);
   }
 }
 
+void PIDSubsystem::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
+
 void PIDSubsystem::Enable() {
   m_controller.Reset();
   m_enabled = true;
 }
 
 void PIDSubsystem::Disable() {
-  UseOutput(0);
+  UseOutput(0, 0);
   m_enabled = false;
 }
 
+bool PIDSubsystem::IsEnabled() { return m_enabled; }
+
 PIDController& PIDSubsystem::GetController() { return m_controller; }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -24,31 +24,18 @@ class PIDSubsystem : public SubsystemBase {
    * Creates a new PIDSubsystem.
    *
    * @param controller the PIDController to use
+   * @param initialPosition the initial setpoint of the subsystem
    */
-  explicit PIDSubsystem(PIDController controller);
+  explicit PIDSubsystem(PIDController controller, double initialPosition = 0);
 
   void Periodic() override;
 
   /**
-   * Uses the output from the PIDController.
+   * Sets the setpoint for the subsystem.
    *
-   * @param output the output of the PIDController
+   * @param setpoint the setpoint for the subsystem
    */
-  virtual void UseOutput(double output) = 0;
-
-  /**
-   * Returns the reference (setpoint) used by the PIDController.
-   *
-   * @return the reference (setpoint) to be used by the controller
-   */
-  virtual double GetSetpoint() = 0;
-
-  /**
-   * Returns the measurement of the process variable used by the PIDController.
-   *
-   * @return the measurement of the process variable
-   */
-  virtual double GetMeasurement() = 0;
+  void SetSetpoint(double setpoint);
 
   /**
    * Enables the PID control.  Resets the controller.
@@ -60,6 +47,13 @@ class PIDSubsystem : public SubsystemBase {
    */
   virtual void Disable();
 
+  /**
+   * Returns whether the controller is enabled.
+   *
+   * @return Whether the controller is enabled.
+   */
+  bool IsEnabled();
+
   /**
    * Returns the PIDController.
    *
@@ -69,6 +63,24 @@ class PIDSubsystem : public SubsystemBase {
 
  protected:
   PIDController m_controller;
-  bool m_enabled;
+  bool m_enabled{false};
+
+  /**
+   * Returns the measurement of the process variable used by the PIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  virtual double GetMeasurement() = 0;
+
+  /**
+   * Uses the output from the PIDController.
+   *
+   * @param output the output of the PIDController
+   * @param setpoint the setpoint of the PIDController (for feedforward)
+   */
+  virtual void UseOutput(double output, double setpoint) = 0;
+
+ private:
+  double m_setpoint{0};
 };
 }  // namespace frc2

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"
@@ -47,15 +48,15 @@ class ProfiledPIDCommand
    * @param requirements      the subsystems required by this command
    */
   ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
-                     std::function<units::unit_t<Distance>> measurementSource,
+                     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)} {
-    AddRequirements(requirements);
+    this->AddRequirements(requirements);
   }
 
   /**
@@ -69,13 +70,56 @@ class ProfiledPIDCommand
    * @param requirements      the subsystems required by this command
    */
   ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
-                     std::function<units::unit_t<Distance>> measurementSource,
-                     std::function<units::unit_t<Distance>> goalSource,
+                     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)},
+        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<Distance_t()> goalSource,
                      std::function<void(double, State)> useOutput,
                      std::initializer_list<Subsystem*> requirements)
       : ProfiledPIDCommand(controller, measurementSource,
                            [&goalSource]() {
-                             return State{goalSource(), 0_mps};
+                             return State{goalSource(), Velocity_t{0}};
+                           },
+                           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) {}
 
@@ -90,8 +134,8 @@ class ProfiledPIDCommand
    * @param requirements      the subsystems required by this command
    */
   ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
-                     std::function<units::unit_t<Distance>> measurementSource,
-                     State goal, std::function<void(double, State)> useOutput,
+                     std::function<Distance_t()> measurementSource, State goal,
+                     std::function<void(double, State)> useOutput,
                      std::initializer_list<Subsystem*> requirements)
       : ProfiledPIDCommand(controller, measurementSource,
                            [goal] { return goal; }, useOutput, requirements) {}
@@ -107,18 +151,53 @@ class ProfiledPIDCommand
    * @param requirements      the subsystems required by this command
    */
   ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
-                     std::function<units::unit_t<Distance>> measurementSource,
-                     units::meter_t goal,
+                     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.
+   *
+   * @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,
                      std::initializer_list<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.
+   *
+   * @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()),
@@ -138,7 +217,7 @@ class ProfiledPIDCommand
 
  protected:
   frc::ProfiledPIDController<Distance> m_controller;
-  std::function<Distance_t> m_measurement;
+  std::function<Distance_t()> m_measurement;
   std::function<State()> m_goal;
   std::function<void(double, State)> m_useOutput;
 };

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -32,46 +32,40 @@ class ProfiledPIDSubsystem : public SubsystemBase {
    * Creates a new ProfiledPIDSubsystem.
    *
    * @param controller the ProfiledPIDController to use
+   * @param initialPosition the initial goal position of the subsystem
    */
-  explicit ProfiledPIDSubsystem(frc::ProfiledPIDController<Distance> controller)
-      : m_controller{controller} {}
+  explicit ProfiledPIDSubsystem(frc::ProfiledPIDController<Distance> controller,
+                                Distance_t initialPosition = Distance_t{0})
+      : m_controller{controller} {
+    SetGoal(initialPosition);
+  }
 
   void Periodic() override {
     if (m_enabled) {
-      UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()),
+      UseOutput(m_controller.Calculate(GetMeasurement(), m_goal),
                 m_controller.GetSetpoint());
     }
   }
 
   /**
-   * Uses the output from the ProfiledPIDController.
+   * Sets the goal state for the subsystem.
    *
-   * @param output the output of the ProfiledPIDController
-   * @param setpoint the setpoint state of the ProfiledPIDController, for
-   * feedforward
+   * @param goal The goal state for the subsystem's motion profile.
    */
-  virtual void UseOutput(double output, State setpoint) = 0;
+  void SetGoal(State goal) { m_goal = goal; }
 
   /**
-   * Returns the goal used by the ProfiledPIDController.
+   * Sets the goal state for the subsystem.  Goal velocity assumed to be zero.
    *
-   * @return the goal to be used by the controller
+   * @param goal The goal position for the subsystem's motion profile.
    */
-  virtual State GetGoal() = 0;
+  void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
 
   /**
-   * Returns the measurement of the process variable used by the
-   * ProfiledPIDController.
-   *
-   * @return the measurement of the process variable
-   */
-  virtual Distance_t GetMeasurement() = 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;
   }
 
@@ -83,6 +77,13 @@ class ProfiledPIDSubsystem : public SubsystemBase {
     m_enabled = false;
   }
 
+  /**
+   * Returns whether the controller is enabled.
+   *
+   * @return Whether the controller is enabled.
+   */
+  bool IsEnabled() { return m_enabled; }
+
   /**
    * Returns the ProfiledPIDController.
    *
@@ -92,6 +93,26 @@ class ProfiledPIDSubsystem : public SubsystemBase {
 
  protected:
   frc::ProfiledPIDController<Distance> m_controller;
-  bool m_enabled;
+  bool m_enabled{false};
+
+  /**
+   * Returns the measurement of the process variable used by the
+   * ProfiledPIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  virtual Distance_t GetMeasurement() = 0;
+
+  /**
+   * Uses the output from the ProfiledPIDController.
+   *
+   * @param output the output of the ProfiledPIDController
+   * @param setpoint the setpoint state of the ProfiledPIDController, for
+   * feedforward
+   */
+  virtual void UseOutput(double output, State setpoint) = 0;
+
+ private:
+  State m_goal;
 };
 }  // namespace frc2

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"
@@ -46,7 +47,7 @@ class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
   /**
    * Constructs a new RamseteCommand that, when executed, will follow the
    * provided trajectory. PID control and feedforward are handled internally,
-   * and outputs are scaled -1 to 1 for easy consumption by speed controllers.
+   * 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
@@ -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
@@ -93,7 +131,7 @@ class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
    * trajectory.
    * @param kinematics      The kinematics for the robot drivetrain.
    * @param output          A function that consumes the computed left and right
-   * outputs (in volts) for the robot drive.
+   * wheel speeds.
    * @param requirements    The subsystems to require.
    */
   RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
@@ -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;