Use __has_include for WPILib.h (#2164)

Now that commands and cameraserver libraries are no longer direct dependencies,
it's necessary to check for their presence.

.wpilib/wpilib_preferences.json

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

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"
 }

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

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/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/native/athena/AddressableLED.cpp

@@ -9,8 +9,6 @@
 
 #include <cstring>
 
-#include <FRC_FPGA_ChipObject/fpgainterfacecapi/NiFpga_HMB.h>
-
 #include "ConstantsInternal.h"
 #include "DigitalInternal.h"
 #include "HALInitializer.h"

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,12 @@ int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) {
   return spiSystem->readTransferSkippedFullCount(status);
 }
 
+// These 2 functions are so the new stall functionality
+// can be tested. How they're used is not very clear
+// but I want them to be testable so we can add an impl.
+// We will not be including these in the headers
+void* HAL_GetSPIDMAManager() { return spiAutoDMA.get(); }
+
+void* HAL_GetSPISystem() { return spiSystem.get(); }
+
 }  // extern "C"

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/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/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')
                     }
             }
         }

shared/config.gradle

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

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')
                 }
             }
         }

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/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/PIDSubsystem.java

@@ -19,6 +19,8 @@ public abstract class PIDSubsystem extends SubsystemBase {
   protected final PIDController m_controller;
   protected boolean m_enabled;
 
+  private double m_setpoint;
+
   /**
    * Creates a new PIDSubsystem.
    *
@@ -32,7 +34,7 @@ public abstract class PIDSubsystem extends SubsystemBase {
   @Override
   public void periodic() {
     if (m_enabled) {
-      useOutput(m_controller.calculate(getMeasurement(), getSetpoint()));
+      useOutput(m_controller.calculate(getMeasurement(), m_setpoint), m_setpoint);
     }
   }
 
@@ -40,26 +42,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 +79,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/ProfiledPIDSubsystem.java

@@ -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,6 +21,8 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
   protected final ProfiledPIDController m_controller;
   protected boolean m_enabled;
 
+  private TrapezoidProfile.State m_goal;
+
   /**
    * Creates a new ProfiledPIDSubsystem.
    *
@@ -33,7 +36,7 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
   @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,27 +44,38 @@ 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.
@@ -78,4 +92,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/TrapezoidProfileSubsystem.java

@@ -18,6 +18,7 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
   private final TrapezoidProfile.Constraints m_constraints;
 
   private TrapezoidProfile.State m_state;
+  private TrapezoidProfile.State m_goal;
 
   /**
    * Creates a new TrapezoidProfileSubsystem.
@@ -51,17 +52,28 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
 
   @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);
   }
 
   /**
-   * 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));
+  }
 
   /**
    * Users should override this to consume the current state of the motion profile.

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

@@ -36,4 +36,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

@@ -14,18 +14,22 @@ PIDSubsystem::PIDSubsystem(PIDController controller)
 
 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/include/frc2/command/PIDCommand.h

@@ -72,7 +72,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

@@ -30,25 +30,11 @@ class PIDSubsystem : public SubsystemBase {
   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 +46,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.
    *
@@ -70,5 +63,23 @@ class PIDSubsystem : public SubsystemBase {
  protected:
   PIDController m_controller;
   bool m_enabled;
+
+  /**
+   * 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

@@ -47,7 +47,7 @@ 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 = {})
@@ -55,7 +55,7 @@ class ProfiledPIDCommand
         m_measurement{std::move(measurementSource)},
         m_goal{std::move(goalSource)},
         m_useOutput{std::move(useOutput)} {
-    AddRequirements(requirements);
+    this->AddRequirements(requirements);
   }
 
   /**
@@ -69,13 +69,13 @@ 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<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) {}
 
@@ -90,8 +90,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,8 +107,8 @@ 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,
+                     Distance_t goal,
                      std::function<void(double, State)> useOutput,
                      std::initializer_list<Subsystem*> requirements)
       : ProfiledPIDCommand(controller, measurementSource,
@@ -138,7 +138,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

@@ -38,34 +38,24 @@ class ProfiledPIDSubsystem : public SubsystemBase {
 
   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;
-
-  /**
-   * Returns the measurement of the process variable used by the
-   * ProfiledPIDController.
-   *
-   * @return the measurement of the process variable
-   */
-  virtual Distance_t GetMeasurement() = 0;
+  void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
 
   /**
    * Enables the PID control.  Resets the controller.
@@ -83,6 +73,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 +89,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

@@ -46,7 +46,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
@@ -93,7 +93,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,

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

@@ -44,7 +44,7 @@ class TrapezoidProfileCommand
                           std::function<void(State)> output,
                           std::initializer_list<Subsystem*> requirements)
       : m_profile(profile), m_output(output) {
-    AddRequirements(requirements);
+    this->AddRequirements(requirements);
   }
 
   void Initialize() override {

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

@@ -41,22 +41,29 @@ class TrapezoidProfileSubsystem : public SubsystemBase {
                             units::second_t period = 20_ms)
       : m_constraints(constraints),
         m_state{position, Velocity_t(0)},
+        m_goal{position, Velocity_t{0}},
         m_period(period) {}
 
   void Periodic() override {
     auto profile =
-        frc::TrapezoidProfile<Distance>(m_constraints, GetGoal(), m_state);
+        frc::TrapezoidProfile<Distance>(m_constraints, m_goal, m_state);
     m_state = profile.Calculate(m_period);
     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.
    */
-  virtual State GetGoal() = 0;
+  void SetGoal(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.
+   */
+  void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
 
  protected:
   /**
@@ -70,6 +77,7 @@ class TrapezoidProfileSubsystem : public SubsystemBase {
  private:
   Constraints m_constraints;
   State m_state;
+  State m_goal;
   units::second_t m_period;
 };
 }  // namespace frc2

wpilibOldCommands/build.gradle

@@ -62,7 +62,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')
             }
         }
     }

wpilibc/build.gradle

@@ -155,7 +155,7 @@ model {
                 lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                 nativeUtils.useRequiredLibrary(it, 'opencv_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')
                 }
             }
         }
@@ -205,7 +205,7 @@ model {
             nativeUtils.useRequiredLibrary(it, 'opencv_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')
             }
         }
     }

wpilibc/src/main/native/cpp/AddressableLED.cpp

@@ -92,3 +92,38 @@ void AddressableLED::Stop() {
   HAL_StopAddressableLEDOutput(m_handle, &status);
   wpi_setHALError(status);
 }
+
+void AddressableLED::LEDData::SetHSV(int h, int s, int v) {
+  if (s == 0) {
+    SetRGB(v, v, v);
+    return;
+  }
+
+  int region = h / 30;
+  int remainder = (h - (region * 30)) * 6;
+
+  int p = (v * (255 - s)) >> 8;
+  int q = (v * (255 - ((s * remainder) >> 8))) >> 8;
+  int t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;
+
+  switch (region) {
+    case 0:
+      SetRGB(v, t, p);
+      break;
+    case 1:
+      SetRGB(q, v, p);
+      break;
+    case 2:
+      SetRGB(p, v, t);
+      break;
+    case 3:
+      SetRGB(p, q, v);
+      break;
+    case 4:
+      SetRGB(t, p, v);
+      break;
+    default:
+      SetRGB(v, p, q);
+      break;
+  }
+}

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

@@ -8,6 +8,7 @@
 #include "frc/DutyCycleEncoder.h"
 
 #include "frc/Counter.h"
+#include "frc/DigitalInput.h"
 #include "frc/DigitalSource.h"
 #include "frc/DriverStation.h"
 #include "frc/DutyCycle.h"
@@ -15,6 +16,12 @@
 
 using namespace frc;
 
+DutyCycleEncoder::DutyCycleEncoder(int channel)
+    : m_dutyCycle{std::make_shared<DutyCycle>(
+          std::make_shared<DigitalInput>(channel))},
+      m_analogTrigger{m_dutyCycle.get()},
+      m_counter{} {}
+
 DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle)
     : m_dutyCycle{&dutyCycle, NullDeleter<DutyCycle>{}},
       m_analogTrigger{m_dutyCycle.get()},

wpilibc/src/main/native/cpp/LinearFilter.cpp

@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/LinearFilter.h"
-
-#include <cassert>
-#include <cmath>
-
-#include <hal/FRCUsageReporting.h>
-
-using namespace frc;
-
-LinearFilter::LinearFilter(wpi::ArrayRef<double> ffGains,
-                           wpi::ArrayRef<double> fbGains)
-    : m_inputs(ffGains.size()),
-      m_outputs(fbGains.size()),
-      m_inputGains(ffGains),
-      m_outputGains(fbGains) {
-  static int instances = 0;
-  instances++;
-  HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances);
-}
-
-LinearFilter LinearFilter::SinglePoleIIR(double timeConstant,
-                                         units::second_t period) {
-  double gain = std::exp(-period.to<double>() / timeConstant);
-  return LinearFilter(1.0 - gain, -gain);
-}
-
-LinearFilter LinearFilter::HighPass(double timeConstant,
-                                    units::second_t period) {
-  double gain = std::exp(-period.to<double>() / timeConstant);
-  return LinearFilter({gain, -gain}, {-gain});
-}
-
-LinearFilter LinearFilter::MovingAverage(int taps) {
-  assert(taps > 0);
-
-  std::vector<double> gains(taps, 1.0 / taps);
-  return LinearFilter(gains, {});
-}
-
-void LinearFilter::Reset() {
-  m_inputs.reset();
-  m_outputs.reset();
-}
-
-double LinearFilter::Calculate(double input) {
-  double retVal = 0.0;
-
-  // Rotate the inputs
-  m_inputs.push_front(input);
-
-  // Calculate the new value
-  for (size_t i = 0; i < m_inputGains.size(); i++) {
-    retVal += m_inputs[i] * m_inputGains[i];
-  }
-  for (size_t i = 0; i < m_outputGains.size(); i++) {
-    retVal -= m_outputs[i] * m_outputGains[i];
-  }
-
-  // Rotate the outputs
-  m_outputs.push_front(retVal);
-
-  return retVal;
-}

wpilibc/src/main/native/cpp/PIDBase.cpp

@@ -35,7 +35,7 @@ PIDBase::PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource& source,
   m_F = Kf;
 
   m_pidInput = &source;
-  m_filter = LinearFilter::MovingAverage(1);
+  m_filter = LinearFilter<double>::MovingAverage(1);
 
   m_pidOutput = &output;
 
@@ -196,7 +196,7 @@ void PIDBase::SetPercentTolerance(double percent) {
 
 void PIDBase::SetToleranceBuffer(int bufLength) {
   std::scoped_lock lock(m_thisMutex);
-  m_filter = LinearFilter::MovingAverage(bufLength);
+  m_filter = LinearFilter<double>::MovingAverage(bufLength);
 }
 
 bool PIDBase::OnTarget() const {

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

@@ -10,34 +10,12 @@
 using namespace frc;
 
 DifferentialDriveOdometry::DifferentialDriveOdometry(
-    DifferentialDriveKinematics kinematics, const Rotation2d& gyroAngle,
-    const Pose2d& initialPose)
-    : m_kinematics(kinematics), m_pose(initialPose) {
+    const Rotation2d& gyroAngle, const Pose2d& initialPose)
+    : m_pose(initialPose) {
   m_previousAngle = m_pose.Rotation();
   m_gyroOffset = m_pose.Rotation() - gyroAngle;
 }
 
-const Pose2d& DifferentialDriveOdometry::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& gyroAngle,
-    const DifferentialDriveWheelSpeeds& wheelSpeeds) {
-  units::second_t deltaTime =
-      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
-  m_previousTime = currentTime;
-
-  auto angle = gyroAngle + m_gyroOffset;
-
-  auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
-  static_cast<void>(dtheta);
-
-  auto newPose = m_pose.Exp(
-      {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
-
-  m_previousAngle = angle;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}
-
 const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
                                                 units::meter_t leftDistance,
                                                 units::meter_t rightDistance) {

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

@@ -42,7 +42,7 @@ struct SendableRegistry::Impl {
     }
   };
 
-  wpi::mutex mutex;
+  wpi::recursive_mutex mutex;
 
   wpi::UidVector<std::unique_ptr<Component>, 32> components;
   wpi::DenseMap<void*, UID> componentMap;
@@ -310,7 +310,9 @@ void SendableRegistry::ForeachLiveWindow(
     wpi::function_ref<void(CallbackData& data)> callback) const {
   assert(dataHandle >= 0);
   std::scoped_lock lock(m_impl->mutex);
-  for (auto&& comp : m_impl->components) {
+  wpi::SmallVector<Impl::Component*, 128> components;
+  for (auto&& comp : m_impl->components) components.emplace_back(comp.get());
+  for (auto comp : components) {
     if (comp->sendable && comp->liveWindow) {
       if (static_cast<size_t>(dataHandle) >= comp->data.size())
         comp->data.resize(dataHandle + 1);

wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp

@@ -27,10 +27,20 @@ CubicHermiteSpline::CubicHermiteSpline(
   // Populate Row 2 and Row 3 with the derivatives of the equations above.
   // Then populate row 4 and 5 with the second derivatives.
   for (int i = 0; i < 4; i++) {
+    // Here, we are multiplying by (3 - i) to manually take the derivative. The
+    // power of the term in index 0 is 3, index 1 is 2 and so on. To find the
+    // coefficient of the derivative, we can use the power rule and multiply
+    // the existing coefficient by its power.
     m_coefficients.template block<2, 1>(2, i) =
         m_coefficients.template block<2, 1>(0, i) * (3 - i);
+  }
 
+  for (int i = 0; i < 3; i++) {
+    // Here, we are multiplying by (2 - i) to manually take the derivative. The
+    // power of the term in index 0 is 2, index 1 is 1 and so on. To find the
+    // coefficient of the derivative, we can use the power rule and multiply
+    // the existing coefficient by its power.
     m_coefficients.template block<2, 1>(4, i) =
-        m_coefficients.template block<2, 1>(2, i) * (3 - i);
+        m_coefficients.template block<2, 1>(2, i) * (2 - i);
   }
 }

wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp

@@ -27,10 +27,19 @@ QuinticHermiteSpline::QuinticHermiteSpline(
   // Populate Row 2 and Row 3 with the derivatives of the equations above.
   // Then populate row 4 and 5 with the second derivatives.
   for (int i = 0; i < 6; i++) {
+    // Here, we are multiplying by (5 - i) to manually take the derivative. The
+    // power of the term in index 0 is 5, index 1 is 4 and so on. To find the
+    // coefficient of the derivative, we can use the power rule and multiply
+    // the existing coefficient by its power.
     m_coefficients.template block<2, 1>(2, i) =
         m_coefficients.template block<2, 1>(0, i) * (5 - i);
-
+  }
+  for (int i = 0; i < 5; i++) {
+    // Here, we are multiplying by (4 - i) to manually take the derivative. The
+    // power of the term in index 0 is 4, index 1 is 3 and so on. To find the
+    // coefficient of the derivative, we can use the power rule and multiply
+    // the existing coefficient by its power.
     m_coefficients.template block<2, 1>(4, i) =
-        m_coefficients.template block<2, 1>(2, i) * (5 - i);
+        m_coefficients.template block<2, 1>(2, i) * (4 - i);
   }
 }

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

@@ -35,12 +35,25 @@ class AddressableLED : public ErrorBase {
 
     /**
      * A helper method to set all values of the LED.
+     *
+     * @param r the r value [0-255]
+     * @param g the g value [0-255]
+     * @param b the b value [0-255]
      */
-    void SetLED(int r, int g, int b) {
+    void SetRGB(int r, int g, int b) {
       this->r = r;
       this->g = g;
       this->b = b;
     }
+
+    /**
+     * A helper method to set all values of the LED.
+     *
+     * @param h the h value [0-180]
+     * @param s the s value [0-255]
+     * @param v the v value [0-255]
+     */
+    void SetHSV(int h, int s, int v);
   };
 
   /**

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

@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <type_traits>
+
 #include <hal/AnalogInput.h>
 #include <hal/DMA.h>
 #include <units/units.h>
@@ -104,5 +106,6 @@ class DMASample : public HAL_DMASample {
   }
 };
 
-static_assert(std::is_pod_v<frc::DMASample>, "DMA Sample MUST Be POD");
+static_assert(std::is_standard_layout_v<frc::DMASample>,
+              "frc::DMASample must have standard layout");
 }  // namespace frc

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

@@ -32,6 +32,13 @@ class DutyCycleEncoder : public ErrorBase,
                          public Sendable,
                          public SendableHelper<DutyCycleEncoder> {
  public:
+  /**
+   * Construct a new DutyCycleEncoder on a specific channel.
+   *
+   * @param channel the channel to attach to
+   */
+  explicit DutyCycleEncoder(int channel);
+
   /**
    * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
    *

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

@@ -7,9 +7,12 @@
 
 #pragma once
 
+#include <cassert>
+#include <cmath>
 #include <initializer_list>
 #include <vector>
 
+#include <hal/FRCUsageReporting.h>
 #include <units/units.h>
 #include <wpi/ArrayRef.h>
 #include <wpi/circular_buffer.h>
@@ -66,6 +69,7 @@ namespace frc {
  * Combining this with Note 1 - the impetus is on YOU as a developer to make
  * sure Calculate() gets called at the desired, constant frequency!
  */
+template <class T>
 class LinearFilter {
  public:
   /**
@@ -74,7 +78,15 @@ class LinearFilter {
    * @param ffGains The "feed forward" or FIR gains.
    * @param fbGains The "feed back" or IIR gains.
    */
-  LinearFilter(wpi::ArrayRef<double> ffGains, wpi::ArrayRef<double> fbGains);
+  LinearFilter(wpi::ArrayRef<double> ffGains, wpi::ArrayRef<double> fbGains)
+      : m_inputs(ffGains.size()),
+        m_outputs(fbGains.size()),
+        m_inputGains(ffGains),
+        m_outputGains(fbGains) {
+    static int instances = 0;
+    instances++;
+    HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances);
+  }
 
   /**
    * Create a linear FIR or IIR filter.
@@ -99,8 +111,11 @@ class LinearFilter {
    * @param period       The period in seconds between samples taken by the
    *                     user.
    */
-  static LinearFilter SinglePoleIIR(double timeConstant,
-                                    units::second_t period);
+  static LinearFilter<T> SinglePoleIIR(double timeConstant,
+                                       units::second_t period) {
+    double gain = std::exp(-period.to<double>() / timeConstant);
+    return LinearFilter(1.0 - gain, -gain);
+  }
 
   /**
    * Creates a first-order high-pass filter of the form:<br>
@@ -113,7 +128,10 @@ class LinearFilter {
    * @param period       The period in seconds between samples taken by the
    *                     user.
    */
-  static LinearFilter HighPass(double timeConstant, units::second_t period);
+  static LinearFilter<T> HighPass(double timeConstant, units::second_t period) {
+    double gain = std::exp(-period.to<double>() / timeConstant);
+    return LinearFilter({gain, -gain}, {-gain});
+  }
 
   /**
    * Creates a K-tap FIR moving average filter of the form:<br>
@@ -124,12 +142,20 @@ class LinearFilter {
    * @param taps The number of samples to average over. Higher = smoother but
    *             slower
    */
-  static LinearFilter MovingAverage(int taps);
+  static LinearFilter<T> MovingAverage(int taps) {
+    assert(taps > 0);
+
+    std::vector<double> gains(taps, 1.0 / taps);
+    return LinearFilter(gains, {});
+  }
 
   /**
    * Reset the filter state.
    */
-  void Reset();
+  void Reset() {
+    m_inputs.reset();
+    m_outputs.reset();
+  }
 
   /**
    * Calculates the next value of the filter.
@@ -138,11 +164,29 @@ class LinearFilter {
    *
    * @return The filtered value at this step
    */
-  double Calculate(double input);
+  T Calculate(T input) {
+    T retVal = T(0.0);
+
+    // Rotate the inputs
+    m_inputs.push_front(input);
+
+    // Calculate the new value
+    for (size_t i = 0; i < m_inputGains.size(); i++) {
+      retVal += m_inputs[i] * m_inputGains[i];
+    }
+    for (size_t i = 0; i < m_outputGains.size(); i++) {
+      retVal -= m_outputs[i] * m_outputGains[i];
+    }
+
+    // Rotate the outputs
+    m_outputs.push_front(retVal);
+
+    return retVal;
+  }
 
  private:
-  wpi::circular_buffer<double> m_inputs{0};
-  wpi::circular_buffer<double> m_outputs{0};
+  wpi::circular_buffer<T> m_inputs;
+  wpi::circular_buffer<T> m_outputs;
   std::vector<double> m_inputGains;
   std::vector<double> m_outputGains;
 };

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

@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <algorithm>
+#include <vector>
+
+#include <wpi/Algorithm.h>
+#include <wpi/circular_buffer.h>
+
+namespace frc {
+/**
+ * A class that implements a moving-window median filter.  Useful for reducing
+ * measurement noise, especially with processes that generate occasional,
+ * extreme outliers (such as values from vision processing, LIDAR, or ultrasonic
+ * sensors).
+ */
+template <class T>
+class MedianFilter {
+ public:
+  /**
+   * Creates a new MedianFilter.
+   *
+   * @param size The number of samples in the moving window.
+   */
+  explicit MedianFilter(size_t size) : m_valueBuffer(size), m_size{size} {}
+
+  /**
+   * Calculates the moving-window median for the next value of the input stream.
+   *
+   * @param next The next input value.
+   * @return The median of the moving window, updated to include the next value.
+   */
+  T Calculate(T next) {
+    // Insert next value at proper point in sorted array
+    wpi::insert_sorted(m_orderedValues, next);
+
+    size_t curSize = m_orderedValues.size();
+
+    // If buffer is at max size, pop element off of end of circular buffer
+    // and remove from ordered list
+    if (curSize > m_size) {
+      m_orderedValues.erase(std::find(m_orderedValues.begin(),
+                                      m_orderedValues.end(),
+                                      m_valueBuffer.pop_back()));
+      curSize = curSize - 1;
+    }
+
+    // Add next value to circular buffer
+    m_valueBuffer.push_front(next);
+
+    if (curSize % 2 == 1) {
+      // If size is odd, return middle element of sorted list
+      return m_orderedValues[curSize / 2];
+    } else {
+      // If size is even, return average of middle elements
+      return (m_orderedValues[curSize / 2 - 1] + m_orderedValues[curSize / 2]) /
+             2.0;
+    }
+  }
+
+  /**
+   * Resets the filter, clearing the window of all elements.
+   */
+  void Reset() {
+    m_orderedValues.clear();
+    m_valueBuffer.reset();
+  }
+
+ private:
+  wpi::circular_buffer<T> m_valueBuffer;
+  std::vector<T> m_orderedValues;
+  size_t m_size;
+};
+}  // namespace frc

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

@@ -402,7 +402,7 @@ class PIDBase : public PIDInterface,
   double m_error = 0;
   double m_result = 0;
 
-  LinearFilter m_filter{{}, {}};
+  LinearFilter<double> m_filter{{}, {}};
 };
 
 }  // namespace frc

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

@@ -16,8 +16,10 @@
 
 // clang-format on
 
+#if __has_include(<cameraserver/CameraServer.h>)
 #include <cameraserver/CameraServer.h>
 #include <vision/VisionRunner.h>
+#endif
 
 #include "frc/ADXL345_I2C.h"
 #include "frc/ADXL345_SPI.h"
@@ -82,6 +84,7 @@
 #include "frc/VictorSP.h"
 #include "frc/WPIErrors.h"
 #include "frc/XboxController.h"
+#if __has_include("frc/buttons/InternalButton.h")
 #include "frc/buttons/InternalButton.h"
 #include "frc/buttons/JoystickButton.h"
 #include "frc/buttons/NetworkButton.h"
@@ -95,6 +98,7 @@
 #include "frc/commands/WaitCommand.h"
 #include "frc/commands/WaitForChildren.h"
 #include "frc/commands/WaitUntilCommand.h"
+#endif
 #include "frc/drive/DifferentialDrive.h"
 #include "frc/drive/KilloughDrive.h"
 #include "frc/drive/MecanumDrive.h"

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

@@ -217,9 +217,10 @@ class ProfiledPIDController
    * @param velocityTolerance Velocity error which is tolerable.
    */
   void SetTolerance(
-      double positionTolerance,
-      double velocityTolerance = std::numeric_limits<double>::infinity()) {
-    m_controller.SetTolerance(positionTolerance, velocityTolerance);
+      Distance_t positionTolerance,
+      Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
+    m_controller.SetTolerance(positionTolerance.template to<double>(),
+                              velocityTolerance.template to<double>());
   }
 
   /**

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

@@ -54,6 +54,13 @@ class RamseteController {
    */
   RamseteController(double b, double zeta);
 
+  /**
+   * Construct a Ramsete unicycle controller. The default arguments for
+   * b and zeta of 2.0 and 0.7 have been well-tested to produce desireable
+   * results.
+   */
+  RamseteController() : RamseteController(2.0, 0.7) {}
+
   /**
    * Returns true if the pose error is within tolerance of the reference.
    */

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

@@ -23,33 +23,24 @@ namespace frc {
  * path following. Furthermore, odometry can be used for latency compensation
  * when using computer-vision systems.
  *
- * There are two ways of tracking the robot's position on the field with this
- * class: one involving using encoder velocities and the other involving encoder
- * positions. It is very important that only one type of odometry is used with
- * each instantiation of this class.
- *
- * Note: If you are using the encoder positions / distances method, it is
- * important that you reset your encoders to zero before using this class. Any
- * subsequent pose resets also require the encoders to be reset to zero.
+ * It is important that you reset your encoders to zero before using this class.
+ * Any subsequent pose resets also require the encoders to be reset to zero.
  */
 class DifferentialDriveOdometry {
  public:
   /**
    * Constructs a DifferentialDriveOdometry object.
    *
-   * @param kinematics The differential drive kinematics for your drivetrain.
    * @param gyroAngle The angle reported by the gyroscope.
    * @param initialPose The starting position of the robot on the field.
    */
-  explicit DifferentialDriveOdometry(DifferentialDriveKinematics kinematics,
-                                     const Rotation2d& gyroAngle,
+  explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
                                      const Pose2d& initialPose = Pose2d());
 
   /**
    * Resets the robot's position on the field.
    *
-   * If you are using the encoder distances method instead of the velocity
-   * method, you NEED to reset your encoders (to zero) when calling this method.
+   * You NEED to reset your encoders (to zero) when calling this method.
    *
    * The gyroscope angle does not need to be reset here on the user's robot
    * code. The library automatically takes care of offsetting the gyro angle.
@@ -72,24 +63,6 @@ class DifferentialDriveOdometry {
    */
   const Pose2d& GetPose() const { return m_pose; }
 
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method takes in the current time as
-   * a parameter to calculate period (difference between two timestamps). The
-   * period is used to calculate the change in distance from a velocity. This
-   * also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
-   *
-   * @param currentTime The current time.
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelSpeeds The current wheel speeds.
-   *
-   * @return The new pose of the robot.
-   */
-  const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& gyroAngle,
-                               const DifferentialDriveWheelSpeeds& wheelSpeeds);
-
   /**
    * Updates the robot position on the field using distance measurements from
    * encoders. This method is more numerically accurate than using velocities to
@@ -104,30 +77,9 @@ class DifferentialDriveOdometry {
   const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
                        units::meter_t rightDistance);
 
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method automatically calculates
-   * the current time to calculate period (difference between two timestamps).
-   * The period is used to calculate the change in distance from a velocity.
-   * This also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelSpeeds     The current wheel speeds.
-   *
-   * @return The new pose of the robot.
-   */
-  const Pose2d& Update(const Rotation2d& gyroAngle,
-                       const DifferentialDriveWheelSpeeds& wheelSpeeds) {
-    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
-                          wheelSpeeds);
-  }
-
  private:
-  DifferentialDriveKinematics m_kinematics;
   Pose2d m_pose;
 
-  units::second_t m_previousTime = -1_s;
   Rotation2d m_gyroOffset;
   Rotation2d m_previousAngle;
 

wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h

@@ -48,7 +48,8 @@ class CubicHermiteSpline : public Spline<3> {
   }
 
  private:
-  Eigen::Matrix<double, 6, 4> m_coefficients;
+  Eigen::Matrix<double, 6, 4> m_coefficients =
+      Eigen::Matrix<double, 6, 4>::Zero();
 
   /**
    * Returns the hermite basis matrix for cubic hermite spline interpolation.

wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h

@@ -48,7 +48,8 @@ class QuinticHermiteSpline : public Spline<5> {
   }
 
  private:
-  Eigen::Matrix<double, 6, 6> m_coefficients;
+  Eigen::Matrix<double, 6, 6> m_coefficients =
+      Eigen::Matrix<double, 6, 6>::Zero();
 
   /**
    * Returns the hermite basis matrix for quintic hermite spline interpolation.

wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp

@@ -45,20 +45,20 @@ static double GetData(double t) {
 class LinearFilterNoiseTest
     : public testing::TestWithParam<LinearFilterNoiseTestType> {
  protected:
-  std::unique_ptr<frc::LinearFilter> m_filter;
+  std::unique_ptr<frc::LinearFilter<double>> m_filter;
 
   void SetUp() override {
     switch (GetParam()) {
       case TEST_SINGLE_POLE_IIR: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
-                                             kFilterStep));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+                                                     kFilterStep));
         break;
       }
 
       case TEST_MOVAVG: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::MovingAverage(kMovAvgTaps));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
         break;
       }
     }

wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp

@@ -73,40 +73,41 @@ static double GetPulseData(double t) {
 class LinearFilterOutputTest
     : public testing::TestWithParam<LinearFilterOutputTestType> {
  protected:
-  std::unique_ptr<frc::LinearFilter> m_filter;
+  std::unique_ptr<frc::LinearFilter<double>> m_filter;
   std::function<double(double)> m_data;
   double m_expectedOutput = 0.0;
 
   void SetUp() override {
     switch (GetParam()) {
       case TEST_SINGLE_POLE_IIR: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
-                                             kFilterStep));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+                                                     kFilterStep));
         m_data = GetData;
         m_expectedOutput = kSinglePoleIIRExpectedOutput;
         break;
       }
 
       case TEST_HIGH_PASS: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::HighPass(kHighPassTimeConstant, kFilterStep));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
+                                                kFilterStep));
         m_data = GetData;
         m_expectedOutput = kHighPassExpectedOutput;
         break;
       }
 
       case TEST_MOVAVG: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::MovingAverage(kMovAvgTaps));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
         m_data = GetData;
         m_expectedOutput = kMovAvgExpectedOutput;
         break;
       }
 
       case TEST_PULSE: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::MovingAverage(kMovAvgTaps));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
         m_data = GetPulseData;
         m_expectedOutput = 0.0;
         break;

wpilibc/src/test/native/cpp/MedianFilterTest.cpp

@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/MedianFilter.h"
+#include "gtest/gtest.h"
+
+TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
+  frc::MedianFilter<double> filter{10};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(4);
+
+  EXPECT_EQ(filter.Calculate(1000), 3.5);
+}
+
+TEST(MedianFilterTest, MedianFilterNotFullTestOdd) {
+  frc::MedianFilter<double> filter{10};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(4);
+  filter.Calculate(7);
+
+  EXPECT_EQ(filter.Calculate(1000), 4);
+}
+
+TEST(MedianFilterTest, MedianFilterFullTestEven) {
+  frc::MedianFilter<double> filter{6};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(0);
+  filter.Calculate(5);
+  filter.Calculate(4);
+  filter.Calculate(1000);
+
+  EXPECT_EQ(filter.Calculate(99), 4.5);
+}
+
+TEST(MedianFilterTest, MedianFilterFullTestOdd) {
+  frc::MedianFilter<double> filter{5};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(5);
+  filter.Calculate(4);
+  filter.Calculate(1000);
+
+  EXPECT_EQ(filter.Calculate(99), 5);
+}

wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp

@@ -15,52 +15,8 @@ static constexpr double kEpsilon = 1E-9;
 
 using namespace frc;
 
-TEST(DifferentialDriveOdometry, OneIteration) {
-  DifferentialDriveKinematics kinematics{0.381_m * 2};
-  DifferentialDriveOdometry odometry{kinematics, 0_rad};
-
-  odometry.ResetPosition(Pose2d(), 0_rad);
-  DifferentialDriveWheelSpeeds wheelSpeeds{0.02_mps, 0.02_mps};
-  odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
-  const auto& pose = odometry.UpdateWithTime(1_s, Rotation2d(), wheelSpeeds);
-
-  EXPECT_NEAR(pose.Translation().X().to<double>(), 0.02, kEpsilon);
-  EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
-}
-
-TEST(DifferentialDriveOdometry, QuarterCircle) {
-  DifferentialDriveKinematics kinematics{0.381_m * 2};
-  DifferentialDriveOdometry odometry{kinematics, 0_rad};
-
-  odometry.ResetPosition(Pose2d(), 0_rad);
-  DifferentialDriveWheelSpeeds wheelSpeeds{
-      0.0_mps, units::meters_per_second_t(5 * wpi::math::pi)};
-  odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
-  const auto& pose =
-      odometry.UpdateWithTime(1_s, Rotation2d(90_deg), wheelSpeeds);
-
-  EXPECT_NEAR(pose.Translation().X().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(pose.Translation().Y().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
-}
-
-TEST(DifferentialDriveWheelSpeeds, GyroAngleReset) {
-  DifferentialDriveKinematics kinematics{0.381_m * 2};
-  DifferentialDriveOdometry odometry{kinematics, Rotation2d(90_deg)};
-
-  odometry.UpdateWithTime(0_s, Rotation2d(90_deg), {});
-  const auto& pose =
-      odometry.UpdateWithTime(1_s, Rotation2d(90_deg), {1_mps, 1_mps});
-
-  EXPECT_NEAR(pose.Translation().X().to<double>(), 1.0, kEpsilon);
-  EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
-}
-
 TEST(DifferentialDriveOdometry, EncoderDistances) {
-  DifferentialDriveKinematics kinematics{0.381_m * 2};
-  DifferentialDriveOdometry odometry{kinematics, Rotation2d(45_deg)};
+  DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
 
   const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
                                      units::meter_t(5 * wpi::math::pi));

wpilibcExamples/build.gradle

@@ -93,7 +93,7 @@ model {
                         lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', 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')
                     }
                 }
                 sources {
@@ -147,7 +147,7 @@ model {
                         lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', 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')
                     }
                 }
                 sources {

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

@@ -12,35 +12,44 @@
 #include <frc/smartdashboard/SmartDashboard.h>
 
 class Robot : public frc::TimedRobot {
+  static constexpr int kLength = 60;
+
   // PWM port 0
   // Must be a PWM header, not MXP or DIO
-  frc::AddressableLED m_led{0};
-  std::array<frc::AddressableLED::LEDData, 12> m_ledBuffer;  // Reuse the buffer
-  int m_count = 0;
+  frc::AddressableLED m_led{9};
+  std::array<frc::AddressableLED::LEDData, kLength>
+      m_ledBuffer;  // Reuse the buffer
+  // Store what the last hue of the first pixel is
+  int firstPixelHue = 0;
 
  public:
+  void Rainbow() {
+    // For every pixel
+    for (int i = 0; i < kLength; i++) {
+      // Calculate the hue - hue is easier for rainbows because the color
+      // shape is a circle so only one value needs to precess
+      const auto pixelHue = (firstPixelHue + (i * 180 / kLength)) % 180;
+      // Set the value
+      m_ledBuffer[i].SetHSV(pixelHue, 255, 128);
+    }
+    // Increase by to make the rainbow "move"
+    firstPixelHue += 3;
+    // Check bounds
+    firstPixelHue %= 180;
+  }
+
   void RobotInit() override {
-    // Default to a length of 12, start empty output
+    // Default to a length of 60, start empty output
     // Length is expensive to set, so only set it once, then just update data
-    m_led.SetLength(12);
+    m_led.SetLength(kLength);
     m_led.SetData(m_ledBuffer);
     m_led.Start();
   }
 
   void RobotPeriodic() override {
-    // Zero out all LEDs
-    for (auto& ledData : m_ledBuffer) {
-      ledData.SetLED(0, 0, 0);
-    }
-
-    // Set 1 single LED to red
-    m_ledBuffer[m_count].SetLED(50, 0, 0);
-
-    // Continue moving LED
-    m_count++;
-    if (m_count >= 12) m_count = 0;
-
-    // Buffer must be written to update.
+    // Fill the buffer with a rainbow
+    Rainbow();
+    // Set the LEDs
     m_led.SetData(m_ledBuffer);
   }
 };

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

@@ -18,10 +18,10 @@ ArmSubsystem::ArmSubsystem()
               kP, 0, 0, {kMaxVelocity, kMaxAcceleration})),
       m_motor(kMotorPort),
       m_encoder(kEncoderPorts[0], kEncoderPorts[1]),
-      m_feedforward(kS, kCos, kV, kA),
-      // Start arm at rest in neutral position
-      m_goal{kArmOffset, 0_rad_per_s} {
+      m_feedforward(kS, kCos, kV, kA) {
   m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.to<double>());
+  // Start arm in neutral position
+  SetGoal(State{kArmOffset, 0_rad_per_s});
 }
 
 void ArmSubsystem::UseOutput(double output, State setpoint) {
@@ -32,12 +32,6 @@ void ArmSubsystem::UseOutput(double output, State setpoint) {
   m_motor.SetVoltage(units::volt_t(output) + feedforward);
 }
 
-void ArmSubsystem::SetGoal(units::radian_t goal) {
-  m_goal = State{goal, 0_rad_per_s};
-}
-
-State ArmSubsystem::GetGoal() { return m_goal; }
-
 units::radian_t ArmSubsystem::GetMeasurement() {
   return units::radian_t(m_encoder.GetDistance()) + kArmOffset;
 }

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

@@ -24,16 +24,10 @@ class ArmSubsystem : public frc2::ProfiledPIDSubsystem<units::radians> {
 
   void UseOutput(double output, State setpoint) override;
 
-  void SetGoal(units::radian_t goal);
-
-  State GetGoal() override;
-
   units::radian_t GetMeasurement() override;
 
  private:
   frc::PWMVictorSPX m_motor;
   frc::Encoder m_encoder;
   frc::ArmFeedforward m_feedforward;
-
-  State m_goal;
 };

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

@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+  m_autonomousCommand = m_container.GetAutonomousCommand();
+
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Schedule();
+  }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+  // This makes sure that the autonomous stops running when
+  // teleop starts running. If you want the autonomous to
+  // continue until interrupted by another command, remove
+  // this line or comment it out.
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Cancel();
+    m_autonomousCommand = nullptr;
+  }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "RobotContainer.h"
+
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/button/JoystickButton.h>
+#include <units/units.h>
+
+RobotContainer::RobotContainer() {
+  // Initialize all of your commands and subsystems here
+
+  // Configure the button bindings
+  ConfigureButtonBindings();
+
+  // Set up default drive command
+  m_drive.SetDefaultCommand(frc2::RunCommand(
+      [this] {
+        m_drive.ArcadeDrive(
+            m_driverController.GetY(frc::GenericHID::kLeftHand),
+            m_driverController.GetX(frc::GenericHID::kRightHand));
+      },
+      {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+  // Configure your button bindings here
+
+  // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
+  frc2::JoystickButton(&m_driverController, 1)
+      .WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm});
+
+  // Move the arm to neutral position when the 'B' button is pressed.
+  frc2::JoystickButton(&m_driverController, 1)
+      .WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); },
+                   {&m_arm});
+
+  // While holding the shoulder button, drive at half speed
+  frc2::JoystickButton(&m_driverController, 6)
+      .WhenPressed([this] { m_drive.SetMaxOutput(.5); })
+      .WhenReleased([this] { m_drive.SetMaxOutput(1); });
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+  // Runs the chosen command in autonomous
+  return new frc2::InstantCommand([] {});
+}

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

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "subsystems/ArmSubsystem.h"
+
+#include "Constants.h"
+
+using namespace ArmConstants;
+using State = frc::TrapezoidProfile<units::radians>::State;
+
+ArmSubsystem::ArmSubsystem()
+    : frc2::TrapezoidProfileSubsystem<units::radians>(
+          {kMaxVelocity, kMaxAcceleration}, kArmOffset),
+      m_motor(kMotorPort),
+      m_feedforward(kS, kCos, kV, kA) {
+  m_motor.SetPID(kP, 0, 0);
+}
+
+void ArmSubsystem::UseState(State setpoint) {
+  // Calculate the feedforward from the sepoint
+  units::volt_t feedforward =
+      m_feedforward.Calculate(setpoint.position, setpoint.velocity);
+  // Add the feedforward to the PID output to get the motor output
+  m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+                      setpoint.position.to<double>(), feedforward / 12_V);
+}

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

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+    : m_left1{kLeftMotor1Port},
+      m_left2{kLeftMotor2Port},
+      m_right1{kRightMotor1Port},
+      m_right2{kRightMotor2Port},
+      m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
+      m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+  // Set the distance per pulse for the encoders
+  m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+  // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+  m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+  m_leftEncoder.Reset();
+  m_rightEncoder.Reset();
+}
+
+double DriveSubsystem::GetAverageEncoderDistance() {
+  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+  m_drive.SetMaxOutput(maxOutput);
+}

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

@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <units/units.h>
+#include <wpi/math>
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants.  This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
+
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
+
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
+    // Assumes the encoders are directly mounted on the wheel shafts
+    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+}  // namespace DriveConstants
+
+namespace ArmConstants {
+constexpr int kMotorPort = 4;
+
+constexpr double kP = 1;
+
+// These are fake gains; in actuality these must be determined individually for
+// each robot
+constexpr auto kS = 1_V;
+constexpr auto kCos = 1_V;
+constexpr auto kV = 0.5_V * 1_s / 1_rad;
+constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
+
+constexpr auto kMaxVelocity = 3_rad_per_s;
+constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s);
+
+constexpr int kEncoderPorts[]{4, 5};
+constexpr int kEncoderPPR = 256;
+constexpr auto kEncoderDistancePerPulse = 2.0_rad * wpi::math::pi / kEncoderPPR;
+
+// The offset of the arm from the horizontal in its neutral position,
+// measured from the horizontal
+constexpr auto kArmOffset = 0.5_rad;
+}  // namespace ArmConstants
+
+namespace AutoConstants {
+constexpr auto kAutoTimeoutSeconds = 12_s;
+constexpr auto kAutoShootTimeSeconds = 7_s;
+}  // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+}  // namespace OIConstants

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

@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/SpeedController.h>
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor
+ * controller.
+ *
+ * <p>Has no actual functionality.
+ */
+class ExampleSmartMotorController : public frc::SpeedController {
+ public:
+  enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
+
+  /**
+   * Creates a new ExampleSmartMotorController.
+   *
+   * @param port The port for the controller.
+   */
+  explicit ExampleSmartMotorController(int port) {}
+
+  /**
+   * Example method for setting the PID gains of the smart controller.
+   *
+   * @param kp The proportional gain.
+   * @param ki The integral gain.
+   * @param kd The derivative gain.
+   */
+  void SetPID(double kp, double ki, double kd) {}
+
+  /**
+   * Example method for setting the setpoint of the smart controller in PID
+   * mode.
+   *
+   * @param mode The mode of the PID controller.
+   * @param setpoint The controller setpoint.
+   * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+   */
+  void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+  /**
+   * Places this motor controller in follower mode.
+   *
+   * @param master The master to follow.
+   */
+  void Follow(ExampleSmartMotorController master) {}
+
+  /**
+   * Returns the encoder distance.
+   *
+   * @return The current encoder distance.
+   */
+  double GetEncoderDistance() { return 0; }
+
+  /**
+   * Returns the encoder rate.
+   *
+   * @return The current encoder rate.
+   */
+  double GetEncoderRate() { return 0; }
+
+  /**
+   * Resets the encoder to zero distance.
+   */
+  void ResetEncoder() {}
+
+  void Set(double speed) override {}
+
+  double Get() const override { return 0; }
+
+  void SetInverted(bool isInverted) override {}
+
+  bool GetInverted() const override { return false; }
+
+  void Disable() override {}
+
+  void StopMotor() override {}
+
+  void PIDWrite(double output) override {}
+};

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

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TestPeriodic() override;
+
+ private:
+  // Have it null by default so that if testing teleop it
+  // doesn't have undefined behavior and potentially crash.
+  frc2::Command* m_autonomousCommand = nullptr;
+
+  RobotContainer m_container;
+};

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

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/ConditionalCommand.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/WaitCommand.h>
+#include <frc2/command/WaitUntilCommand.h>
+
+#include "Constants.h"
+#include "subsystems/ArmSubsystem.h"
+#include "subsystems/DriveSubsystem.h"
+
+namespace ac = AutoConstants;
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls).  Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+  RobotContainer();
+
+  frc2::Command* GetAutonomousCommand();
+
+ private:
+  // The driver's controller
+  frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+  // The robot's subsystems and commands are defined here...
+
+  // The robot's subsystems
+  DriveSubsystem m_drive;
+  ArmSubsystem m_arm;
+
+  // The chooser for the autonomous routines
+
+  void ConfigureButtonBindings();
+};

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

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/controller/ArmFeedforward.h>
+#include <frc2/command/TrapezoidProfileSubsystem.h>
+#include <units/units.h>
+
+#include "ExampleSmartMotorController.h"
+
+/**
+ * A robot arm subsystem that moves with a motion profile.
+ */
+class ArmSubsystem : public frc2::TrapezoidProfileSubsystem<units::radians> {
+  using State = frc::TrapezoidProfile<units::radians>::State;
+
+ public:
+  ArmSubsystem();
+
+  void UseState(State setpoint) override;
+
+ private:
+  ExampleSmartMotorController m_motor;
+  frc::ArmFeedforward m_feedforward;
+};

wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h

@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+  DriveSubsystem();
+
+  /**
+   * Will be called periodically whenever the CommandScheduler runs.
+   */
+  void Periodic() override;
+
+  // Subsystem methods go here.
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  void ArcadeDrive(double fwd, double rot);
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  void ResetEncoders();
+
+  /**
+   * Gets the average distance of the TWO encoders.
+   *
+   * @return the average of the TWO encoder readings
+   */
+  double GetAverageEncoderDistance();
+
+  /**
+   * Gets the left drive encoder.
+   *
+   * @return the left drive encoder
+   */
+  frc::Encoder& GetLeftEncoder();
+
+  /**
+   * Gets the right drive encoder.
+   *
+   * @return the right drive encoder
+   */
+  frc::Encoder& GetRightEncoder();
+
+  /**
+   * Sets the max output of the drive.  Useful for scaling the drive to drive
+   * more slowly.
+   *
+   * @param maxOutput the maximum output to which the drive will be constrained
+   */
+  void SetMaxOutput(double maxOutput);
+
+ private:
+  // Components (e.g. motor controllers and sensors) should generally be
+  // declared private and exposed only through public methods.
+
+  // The motor controllers
+  frc::PWMVictorSPX m_left1;
+  frc::PWMVictorSPX m_left2;
+  frc::PWMVictorSPX m_right1;
+  frc::PWMVictorSPX m_right2;
+
+  // The motors on the left side of the drive
+  frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+
+  // The motors on the right side of the drive
+  frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+
+  // The robot's drive
+  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+  // The left-side drive encoder
+  frc::Encoder m_leftEncoder;
+
+  // The right-side drive encoder
+  frc::Encoder m_rightEncoder;
+};

wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp

@@ -7,11 +7,6 @@
 
 #include "Drivetrain.h"
 
-frc::DifferentialDriveWheelSpeeds Drivetrain::GetSpeeds() const {
-  return {units::meters_per_second_t(m_leftEncoder.GetRate()),
-          units::meters_per_second_t(m_rightEncoder.GetRate())};
-}
-
 void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
   const auto leftOutput = m_leftPIDController.Calculate(
       m_leftEncoder.GetRate(), speeds.left.to<double>());
@@ -28,5 +23,6 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
 }
 
 void Drivetrain::UpdateOdometry() {
-  m_odometry.Update(GetAngle(), GetSpeeds());
+  m_odometry.Update(GetAngle(), units::meter_t(m_leftEncoder.GetDistance()),
+                    units::meter_t(m_rightEncoder.GetDistance()));
 }

wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h

@@ -31,6 +31,9 @@ class Drivetrain {
                                       kEncoderResolution);
     m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
                                        kEncoderResolution);
+
+    m_leftEncoder.Reset();
+    m_rightEncoder.Reset();
   }
 
   /**
@@ -46,7 +49,6 @@ class Drivetrain {
   static constexpr units::radians_per_second_t kMaxAngularSpeed{
       wpi::math::pi};  // 1/2 rotation per second
 
-  frc::DifferentialDriveWheelSpeeds GetSpeeds() const;
   void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
   void Drive(units::meters_per_second_t xSpeed,
              units::radians_per_second_t rot);
@@ -74,5 +76,5 @@ class Drivetrain {
   frc::AnalogGyro m_gyro{0};
 
   frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
-  frc::DifferentialDriveOdometry m_odometry{m_kinematics, GetAngle()};
+  frc::DifferentialDriveOdometry m_odometry{GetAngle()};
 };

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

@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+  m_autonomousCommand = m_container.GetAutonomousCommand();
+
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Schedule();
+  }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+  // This makes sure that the autonomous stops running when
+  // teleop starts running. If you want the autonomous to
+  // continue until interrupted by another command, remove
+  // this line or comment it out.
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Cancel();
+    m_autonomousCommand = nullptr;
+  }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp

@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "RobotContainer.h"
+
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/button/JoystickButton.h>
+
+#include "commands/DriveDistanceProfiled.h"
+
+RobotContainer::RobotContainer() {
+  // Initialize all of your commands and subsystems here
+
+  // Configure the button bindings
+  ConfigureButtonBindings();
+
+  // Set up default drive command
+  m_drive.SetDefaultCommand(frc2::RunCommand(
+      [this] {
+        m_drive.ArcadeDrive(
+            m_driverController.GetY(frc::GenericHID::kLeftHand),
+            m_driverController.GetX(frc::GenericHID::kRightHand));
+      },
+      {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+  // Configure your button bindings here
+
+  // While holding the shoulder button, drive at half speed
+  frc2::JoystickButton(&m_driverController, 6)
+      .WhenPressed(&m_driveHalfSpeed)
+      .WhenReleased(&m_driveFullSpeed);
+
+  // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of
+  // 10 seconds
+  frc2::JoystickButton(&m_driverController, 1)
+      .WhenPressed(DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s));
+
+  // Do the same thing as above when the 'B' button is pressed, but defined
+  // inline
+  frc2::JoystickButton(&m_driverController, 2)
+      .WhenPressed(
+          frc2::TrapezoidProfileCommand<units::meters>(
+              frc::TrapezoidProfile<units::meters>(
+                  // Limit the max acceleration and velocity
+                  {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration},
+                  // End at desired position in meters; implicitly starts at 0
+                  {3_m, 0_mps}),
+              // Pipe the profile state to the drive
+              [this](auto setpointState) {
+                m_drive.SetDriveStates(setpointState, setpointState);
+              },
+              // Require the drive
+              {&m_drive})
+              .BeforeStarting([this]() { m_drive.ResetEncoders(); })
+              .WithTimeout(10_s));
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+  // Runs the chosen command in autonomous
+  return new frc2::InstantCommand([] {});
+}

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "commands/DriveDistanceProfiled.h"
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance,
+                                             DriveSubsystem* drive)
+    : CommandHelper(
+          frc::TrapezoidProfile<units::meters>(
+              // Limit the max acceleration and velocity
+              {kMaxSpeed, kMaxAcceleration},
+              // End at desired position in meters; implicitly starts at 0
+              {distance, 0_mps}),
+          // Pipe the profile state to the drive
+          [drive](auto setpointState) {
+            drive->SetDriveStates(setpointState, setpointState);
+          },
+          // Require the drive
+          {drive}) {
+  // Reset drive encoders since we're starting at 0
+  drive->ResetEncoders();
+}

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp

@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+    : m_leftMaster{kLeftMotor1Port},
+      m_leftSlave{kLeftMotor2Port},
+      m_rightMaster{kRightMotor1Port},
+      m_rightSlave{kRightMotor2Port},
+      m_feedforward{ks, kv, ka} {
+  m_leftSlave.Follow(m_leftMaster);
+  m_rightSlave.Follow(m_rightMaster);
+
+  m_leftMaster.SetPID(kp, 0, 0);
+  m_rightMaster.SetPID(kp, 0, 0);
+}
+
+void DriveSubsystem::Periodic() {
+  // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::SetDriveStates(
+    frc::TrapezoidProfile<units::meters>::State left,
+    frc::TrapezoidProfile<units::meters>::State right) {
+  m_leftMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+                           left.position.to<double>(),
+                           m_feedforward.Calculate(left.velocity) / 12_V);
+  m_rightMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+                            right.position.to<double>(),
+                            m_feedforward.Calculate(right.velocity) / 12_V);
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+  m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+  m_leftMaster.ResetEncoder();
+  m_rightMaster.ResetEncoder();
+}
+
+units::meter_t DriveSubsystem::GetLeftEncoderDistance() {
+  return units::meter_t(m_leftMaster.GetEncoderDistance());
+}
+
+units::meter_t DriveSubsystem::GetRightEncoderDistance() {
+  return units::meter_t(m_rightMaster.GetEncoderDistance());
+}
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+  m_drive.SetMaxOutput(maxOutput);
+}

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

@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <units/units.h>
+#include <wpi/math>
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants.  This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
+
+// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+// These characterization values MUST be determined either experimentally or
+// theoretically for *your* robot's drive. The RobotPy Characterization
+// Toolsuite provides a convenient tool for obtaining these values for your
+// robot.
+constexpr auto ks = 1_V;
+constexpr auto kv = 0.8_V * 1_s / 1_m;
+constexpr auto ka = 0.15_V * 1_s * 1_s / 1_m;
+
+constexpr double kp = 1;
+
+constexpr auto kMaxSpeed = 3_mps;
+constexpr auto kMaxAcceleration = 3_mps_sq;
+
+}  // namespace DriveConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+}  // namespace OIConstants

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h

@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/SpeedController.h>
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor
+ * controller.
+ *
+ * <p>Has no actual functionality.
+ */
+class ExampleSmartMotorController : public frc::SpeedController {
+ public:
+  enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
+
+  /**
+   * Creates a new ExampleSmartMotorController.
+   *
+   * @param port The port for the controller.
+   */
+  explicit ExampleSmartMotorController(int port) {}
+
+  /**
+   * Example method for setting the PID gains of the smart controller.
+   *
+   * @param kp The proportional gain.
+   * @param ki The integral gain.
+   * @param kd The derivative gain.
+   */
+  void SetPID(double kp, double ki, double kd) {}
+
+  /**
+   * Example method for setting the setpoint of the smart controller in PID
+   * mode.
+   *
+   * @param mode The mode of the PID controller.
+   * @param setpoint The controller setpoint.
+   * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+   */
+  void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+  /**
+   * Places this motor controller in follower mode.
+   *
+   * @param master The master to follow.
+   */
+  void Follow(ExampleSmartMotorController master) {}
+
+  /**
+   * Returns the encoder distance.
+   *
+   * @return The current encoder distance.
+   */
+  double GetEncoderDistance() { return 0; }
+
+  /**
+   * Returns the encoder rate.
+   *
+   * @return The current encoder rate.
+   */
+  double GetEncoderRate() { return 0; }
+
+  /**
+   * Resets the encoder to zero distance.
+   */
+  void ResetEncoder() {}
+
+  void Set(double speed) override {}
+
+  double Get() const override { return 0; }
+
+  void SetInverted(bool isInverted) override {}
+
+  bool GetInverted() const override { return false; }
+
+  void Disable() override {}
+
+  void StopMotor() override {}
+
+  void PIDWrite(double output) override {}
+};

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TestPeriodic() override;
+
+ private:
+  // Have it null by default so that if testing teleop it
+  // doesn't have undefined behavior and potentially crash.
+  frc2::Command* m_autonomousCommand = nullptr;
+
+  RobotContainer m_container;
+};

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/StartEndCommand.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls).  Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+  RobotContainer();
+
+  frc2::Command* GetAutonomousCommand();
+
+ private:
+  // The driver's controller
+  frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+  // The robot's subsystems and commands are defined here...
+
+  // The robot's subsystems
+  DriveSubsystem m_drive;
+
+  frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
+                                        {}};
+  frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
+                                        {}};
+
+  void ConfigureButtonBindings();
+};

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h

@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/TrapezoidProfileCommand.h>
+
+#include "subsystems/DriveSubsystem.h"
+
+class DriveDistanceProfiled
+    : public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meters>,
+                                 DriveDistanceProfiled> {
+ public:
+  DriveDistanceProfiled(units::meter_t distance, DriveSubsystem* drive);
+};

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h

@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <frc2/command/SubsystemBase.h>
+#include <units/units.h>
+
+#include "Constants.h"
+#include "ExampleSmartMotorController.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+  DriveSubsystem();
+
+  /**
+   * Will be called periodically whenever the CommandScheduler runs.
+   */
+  void Periodic() override;
+
+  // Subsystem methods go here.
+
+  /**
+   * Attempts to follow the given drive states using offboard PID.
+   *
+   * @param left The left wheel state.
+   * @param right The right wheel state.
+   */
+  void SetDriveStates(frc::TrapezoidProfile<units::meters>::State left,
+                      frc::TrapezoidProfile<units::meters>::State right);
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  void ArcadeDrive(double fwd, double rot);
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  void ResetEncoders();
+
+  /**
+   * Gets the distance of the left encoder.
+   *
+   * @return the average of the TWO encoder readings
+   */
+  units::meter_t GetLeftEncoderDistance();
+
+  /**
+   * Gets the distance of the right encoder.
+   *
+   * @return the average of the TWO encoder readings
+   */
+  units::meter_t GetRightEncoderDistance();
+
+  /**
+   * Sets the max output of the drive.  Useful for scaling the drive to drive
+   * more slowly.
+   *
+   * @param maxOutput the maximum output to which the drive will be constrained
+   */
+  void SetMaxOutput(double maxOutput);
+
+ private:
+  // Components (e.g. motor controllers and sensors) should generally be
+  // declared private and exposed only through public methods.
+
+  // The motor controllers
+  ExampleSmartMotorController m_leftMaster;
+  ExampleSmartMotorController m_leftSlave;
+  ExampleSmartMotorController m_rightMaster;
+  ExampleSmartMotorController m_rightSlave;
+
+  // A feedforward component for the drive
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward;
+
+  // The robot's drive
+  frc::DifferentialDrive m_drive{m_leftMaster, m_rightMaster};
+};

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

@@ -5,17 +5,13 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include <frc/DigitalInput.h>
-#include <frc/DutyCycle.h>
 #include <frc/DutyCycleEncoder.h>
 #include <frc/TimedRobot.h>
 #include <frc/smartdashboard/SmartDashboard.h>
 
 class Robot : public frc::TimedRobot {
-  frc::DigitalInput m_input{0};  // Input channel
-
-  // Duty cycle encoder
-  frc::DutyCycleEncoder m_dutyCycleEncoder{m_input};
+  // Duty cycle encoder on channel 0
+  frc::DutyCycleEncoder m_dutyCycleEncoder{0};
 
  public:
   void RobotInit() override {