[wpimath] DCMotor: fix doc typo (NFC) (#2868)

This builds sim extensions as dylib's instead of so's on macOS.

.github/workflows/ci.yml

@@ -8,7 +8,7 @@ jobs:
       fail-fast: false
       matrix:
         include:
-          - container: wpilib/roborio-cross-ubuntu:2020-18.04
+          - container: wpilib/roborio-cross-ubuntu:2021-18.04
             artifact-name: Athena
             build-options: "-Ponlylinuxathena"
           - container: wpilib/raspbian-cross-ubuntu:10-18.04
@@ -101,8 +101,11 @@ jobs:
       - uses: actions/setup-java@v1
         with:
           java-version: 13
+      - name: Set release environment variable
+        run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
+        if: startsWith(github.ref, 'refs/tags/v')
       - name: Build with Gradle
-        run: ./gradlew docs:zipDocs
+        run: ./gradlew docs:zipDocs -PbuildServer ${{ env.EXTRA_GRADLE_ARGS }}
       - uses: actions/upload-artifact@v2
         with:
           name: Documentation
@@ -156,7 +159,7 @@ jobs:
         uses: lukka/run-cmake@v3
         with:
           buildDirectory: ${{ runner.workspace }}/build/
-          cmakeAppendedArgs: -DWITH_JAVA=OFF -DWITH_GUI=OFF
+          cmakeAppendedArgs: -DWITH_JAVA=OFF
           cmakeListsOrSettingsJson: CMakeListsTxtAdvanced
           useVcpkgToolchainFile: true
       - name: Run Tests

CONTRIBUTING.md

@@ -1,6 +1,6 @@
 # Contributing to WPILib
 
-So you want to contribute your changes back to WPILib. Great! We have a few contributing rules that will help you make sure your changes will be accepted into the project. Please remember to follow the rules written here, and behave with Gracious Professionalism.
+So you want to contribute your changes back to WPILib. Great! We have a few contributing rules that will help you make sure your changes will be accepted into the project. Please remember to follow the rules in the [code of conduct](https://github.com/wpilibsuite/allwpilib/blob/master/CODE_OF_CONDUCT.md), and behave with Gracious Professionalism.
 
 - [General Contribution Rules](#general-contribution-rules)
 - [What to Contribute](#what-to-contribute)
@@ -37,7 +37,7 @@ So you want to contribute your changes back to WPILib. Great! We have a few cont
 
 ## Coding Guidelines
 
-WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system. We currently use clang-format 6.0 with wpiformat.
+WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system. We currently use clang-format 10.0 with wpiformat.
 
 While the library should be fully formatted according to the styles, additional elements of the style guide were not followed when the library was initially created. All new code should follow the guidelines. If you are looking for some easy ramp-up tasks, finding areas that don't follow the style guide and fixing them is very welcome.
 
@@ -45,11 +45,11 @@ While the library should be fully formatted according to the styles, additional
 
 ### Pull Request Format
 
-Changes should be submitted as a Pull Request against the master branch of WPILib. For most changes, we ask that you squash your changes down to a single commit. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. No change will be merged unless it is up to date with the current master. We will also not merge any changes with merge commits in them; please rebase off of master before submitting a pull request. We do this to make sure that the git history isn't too cluttered.
+Changes should be submitted as a Pull Request against the master branch of WPILib. For most changes, commits will be squashed upon merge. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. We may ask you to break a PR into multiple standalone PRs or commits for rebase within one PR to separate unrelated changes. No change will be merged unless it is up to date with the current master. We do this to make sure that the git history isn't too cluttered.
 
 ### Merge Process
 
-When you first submit changes, Azure DevOps will attempt to run `./gradlew check` on your change. If this fails, you will need to fix any issues that it sees. Once Azure passes, we will begin the review process in more earnest. One or more WPILib team members will review your change. This will be a back-and-forth process with the WPILib team and the greater community. Once we are satisfied that your change is ready, we will allow our hosted instance to test it. This will run the full gamut of checks, including integration tests on actual hardware. Once all tests have passed and the team is satisfied, we will merge your change into the WPILib repository.
+When you first submit changes, GitHub Actions will attempt to run `./gradlew check` on your change. If this fails, you will need to fix any issues that it sees. Once Actions passes, we will begin the review process in more earnest. One or more WPILib team members will review your change. This will be a back-and-forth process with the WPILib team and the greater community. Once we are satisfied that your change is ready, we will allow our hosted instance to test it. This will run the full gamut of checks, including integration tests on actual hardware. Once all tests have passed and the team is satisfied, we will merge your change into the WPILib repository.
 
 ## Licensing
 

OtherVersions.md

@@ -58,7 +58,8 @@ wpi.wpilibVersion = 'YEAR.424242.+'
 C++
 ```groovy
 plugins {
-  id "java"
+  id "cpp"
+  id "google-test-test-suite"
   id "edu.wpi.first.GradleRIO" version "2020.3.2"
 }
 

azure-pipelines-testbench.yaml

@@ -15,7 +15,7 @@ stages:
       vmImage: 'Ubuntu 16.04'
 
     container:
-      image:  wpilib/roborio-cross-ubuntu:2020-18.04
+      image:  wpilib/roborio-cross-ubuntu:2021-18.04
 
     timeoutInMinutes: 0
 

buildSrc/build.gradle

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

cameraserver/cameraserver-config.cmake.in

@@ -5,4 +5,5 @@ include(CMakeFindDependencyMacro)
 @CSCORE_DEP_REPLACE@
 find_dependency(OpenCV)
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/cameraserver.cmake)

cmake/toolchains/arm-frc-gnueabi.toolchain.cmake

@@ -1,4 +1,4 @@
 set(GCC_COMPILER_VERSION "" CACHE STRING "GCC Compiler version")
-set(GNU_MACHINE "arm-frc2020-linux-gnueabi" CACHE STRING "GNU compiler triple")
+set(GNU_MACHINE "arm-frc2021-linux-gnueabi" CACHE STRING "GNU compiler triple")
 set(SOFTFP yes)
 include("${CMAKE_CURRENT_LIST_DIR}/arm.toolchain.cmake")

cscore/cscore-config.cmake.in

@@ -3,4 +3,5 @@ include(CMakeFindDependencyMacro)
 @WPIUTIL_DEP_REPLACE@
 find_dependency(OpenCV)
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/cscore.cmake)

hal/hal-config.cmake.in

@@ -2,4 +2,5 @@ include(CMakeFindDependencyMacro)
 @FILENAME_DEP_REPLACE@
 @WPIUTIL_DEP_REPLACE@
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/hal.cmake)

hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceDataJNI.java

@@ -17,7 +17,7 @@ public class SimDeviceDataJNI extends JNIWrapper {
   public static native int registerSimDeviceCreatedCallback(String prefix, SimDeviceCallback callback, boolean initialNotify);
   public static native void cancelSimDeviceCreatedCallback(int uid);
 
-  public static native int registerSimDeviceFreedCallback(String prefix, SimDeviceCallback callback);
+  public static native int registerSimDeviceFreedCallback(String prefix, SimDeviceCallback callback, boolean initialNotify);
   public static native void cancelSimDeviceFreedCallback(int uid);
 
   public static native int getSimDeviceHandle(String name);

hal/src/main/native/athena/mockdata/SimDeviceData.cpp

@@ -23,8 +23,9 @@ int32_t HALSIM_RegisterSimDeviceCreatedCallback(
 
 void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid) {}
 
-int32_t HALSIM_RegisterSimDeviceFreedCallback(
-    const char* prefix, void* param, HALSIM_SimDeviceCallback callback) {
+int32_t HALSIM_RegisterSimDeviceFreedCallback(const char* prefix, void* param,
+                                              HALSIM_SimDeviceCallback callback,
+                                              HAL_Bool initialNotify) {
   return 0;
 }
 

hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.cpp

@@ -9,15 +9,13 @@
 
 #include <jni.h>
 
-#include <functional>
-#include <string>
 #include <utility>
 
-#include <wpi/UidVector.h>
 #include <wpi/jni_util.h>
 
 #include "SimulatorJNI.h"
 #include "edu_wpi_first_hal_simulation_SimDeviceDataJNI.h"
+#include "hal/handles/UnlimitedHandleResource.h"
 #include "hal/simulation/SimDeviceData.h"
 
 using namespace hal;
@@ -39,7 +37,6 @@ struct DeviceInfo {
   HAL_SimValueHandle handle;
 
   jobject MakeJava(JNIEnv* env) const;
-  void CallJava(JNIEnv* env, jobject callobj) const;
 };
 
 struct ValueInfo {
@@ -52,10 +49,6 @@ struct ValueInfo {
   HAL_Value value;
 
   jobject MakeJava(JNIEnv* env) const;
-  void CallJava(JNIEnv* env, jobject callobj) const;
-
- private:
-  std::pair<jlong, jdouble> ToValue12() const;
 };
 
 }  // namespace
@@ -67,12 +60,7 @@ jobject DeviceInfo::MakeJava(JNIEnv* env) const {
                         (jint)handle);
 }
 
-void DeviceInfo::CallJava(JNIEnv* env, jobject callobj) const {
-  env->CallVoidMethod(callobj, simDeviceCallbackCallback,
-                      MakeJString(env, name), (jint)handle);
-}
-
-std::pair<jlong, jdouble> ValueInfo::ToValue12() const {
+static std::pair<jlong, jdouble> ToValue12(const HAL_Value& value) {
   jlong value1 = 0;
   jdouble value2 = 0.0;
   switch (value.type) {
@@ -100,164 +88,213 @@ std::pair<jlong, jdouble> ValueInfo::ToValue12() const {
 jobject ValueInfo::MakeJava(JNIEnv* env) const {
   static jmethodID func =
       env->GetMethodID(simValueInfoCls, "<init>", "(Ljava/lang/String;IZIJD)V");
-  auto [value1, value2] = ToValue12();
+  auto [value1, value2] = ToValue12(value);
   return env->NewObject(simValueInfoCls, func, MakeJString(env, name),
                         (jint)handle, (jboolean)readonly, (jint)value.type,
                         value1, value2);
 }
 
-void ValueInfo::CallJava(JNIEnv* env, jobject callobj) const {
-  auto [value1, value2] = ToValue12();
-  env->CallVoidMethod(callobj, simValueCallbackCallback, MakeJString(env, name),
-                      (jint)handle, (jboolean)readonly, (jint)value.type,
-                      value1, value2);
-}
-
 namespace {
 
-class CallbackStore {
+class DeviceCallbackStore {
  public:
-  explicit CallbackStore(JNIEnv* env, jobject obj) : m_call{env, obj} {}
-  ~CallbackStore() {
-    if (m_cancelCallback) m_cancelCallback();
-  }
-
-  void SetCancel(std::function<void()> cancelCallback) {
-    m_cancelCallback = std::move(cancelCallback);
-  }
-  void Free(JNIEnv* env) { m_call.free(env); }
-  jobject Get() const { return m_call; }
+  void create(JNIEnv* env, jobject obj) { m_call = JGlobal<jobject>(env, obj); }
+  void performCallback(const char* name, HAL_SimDeviceHandle handle);
+  void free(JNIEnv* env) { m_call.free(env); }
+  void setCallbackId(int32_t id) { callbackId = id; }
+  int32_t getCallbackId() { return callbackId; }
 
  private:
   wpi::java::JGlobal<jobject> m_call;
-  std::function<void()> m_cancelCallback;
+  int32_t callbackId;
 };
 
-class CallbackThreadJNI : public wpi::SafeThread {
+class ValueCallbackStore {
  public:
-  void Main();
-
-  using DeviceCalls =
-      std::vector<std::pair<std::weak_ptr<CallbackStore>, DeviceInfo>>;
-  DeviceCalls m_deviceCalls;
-  using ValueCalls =
-      std::vector<std::pair<std::weak_ptr<CallbackStore>, ValueInfo>>;
-  ValueCalls m_valueCalls;
-
-  wpi::UidVector<std::shared_ptr<CallbackStore>, 4> m_callbacks;
-};
-
-class CallbackJNI {
- public:
-  static CallbackJNI& GetInstance() {
-    static CallbackJNI inst;
-    return inst;
-  }
-  void SendDevice(int32_t callback, DeviceInfo info);
-  void SendValue(int32_t callback, ValueInfo info);
-
-  std::pair<int32_t, std::shared_ptr<CallbackStore>> AllocateCallback(
-      JNIEnv* env, jobject obj);
-
-  void FreeCallback(JNIEnv* env, int32_t uid);
+  void create(JNIEnv* env, jobject obj) { m_call = JGlobal<jobject>(env, obj); }
+  void performCallback(const char* name, HAL_SimValueHandle handle,
+                       bool readonly, const HAL_Value& value);
+  void free(JNIEnv* env) { m_call.free(env); }
+  void setCallbackId(int32_t id) { callbackId = id; }
+  int32_t getCallbackId() { return callbackId; }
 
  private:
-  CallbackJNI() { m_owner.Start(); }
-
-  wpi::SafeThreadOwner<CallbackThreadJNI> m_owner;
+  wpi::java::JGlobal<jobject> m_call;
+  int32_t callbackId;
 };
 
 }  // namespace
 
-void CallbackThreadJNI::Main() {
+void DeviceCallbackStore::performCallback(const char* name,
+                                          HAL_SimDeviceHandle handle) {
   JNIEnv* env;
-  JavaVMAttachArgs args;
-  args.version = JNI_VERSION_1_2;
-  args.name = const_cast<char*>("SimDeviceCallback");
-  args.group = nullptr;
-  jint rs = sim::GetJVM()->AttachCurrentThreadAsDaemon(
-      reinterpret_cast<void**>(&env), &args);
-  if (rs != JNI_OK) return;
-
-  DeviceCalls deviceCalls;
-  ValueCalls valueCalls;
-
-  std::unique_lock lock(m_mutex);
-  while (m_active) {
-    m_cond.wait(lock, [&] { return !m_active; });
-    if (!m_active) break;
-
-    deviceCalls.swap(m_deviceCalls);
-    valueCalls.swap(m_valueCalls);
-
-    lock.unlock();  // don't hold mutex during callback execution
-
-    for (auto&& call : deviceCalls) {
-      if (auto store = call.first.lock()) {
-        if (jobject callobj = store->Get()) {
-          call.second.CallJava(env, callobj);
-          if (env->ExceptionCheck()) {
-            env->ExceptionDescribe();
-            env->ExceptionClear();
-          }
-        }
-      }
+  JavaVM* vm = sim::GetJVM();
+  bool didAttachThread = false;
+  int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
+  if (tryGetEnv == JNI_EDETACHED) {
+    // Thread not attached
+    didAttachThread = true;
+    if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
+      // Failed to attach, log and return
+      wpi::outs() << "Failed to attach\n";
+      wpi::outs().flush();
+      return;
     }
-
-    for (auto&& call : valueCalls) {
-      if (auto store = call.first.lock()) {
-        if (jobject callobj = store->Get()) {
-          call.second.CallJava(env, callobj);
-          if (env->ExceptionCheck()) {
-            env->ExceptionDescribe();
-            env->ExceptionClear();
-          }
-        }
-      }
-    }
-
-    deviceCalls.clear();
-    valueCalls.clear();
-
-    lock.lock();
+  } else if (tryGetEnv == JNI_EVERSION) {
+    wpi::outs() << "Invalid JVM Version requested\n";
+    wpi::outs().flush();
   }
 
-  // free global references
-  for (auto&& callback : m_callbacks) callback->Free(env);
+  env->CallVoidMethod(m_call, simDeviceCallbackCallback, MakeJString(env, name),
+                      (jint)handle);
 
-  sim::GetJVM()->DetachCurrentThread();
+  if (env->ExceptionCheck()) {
+    env->ExceptionDescribe();
+  }
+
+  if (didAttachThread) {
+    vm->DetachCurrentThread();
+  }
 }
 
-void CallbackJNI::SendDevice(int32_t callback, DeviceInfo info) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-  thr->m_deviceCalls.emplace_back(thr->m_callbacks[callback], std::move(info));
-  thr->m_cond.notify_one();
+void ValueCallbackStore::performCallback(const char* name,
+                                         HAL_SimValueHandle handle,
+                                         bool readonly,
+                                         const HAL_Value& value) {
+  JNIEnv* env;
+  JavaVM* vm = sim::GetJVM();
+  bool didAttachThread = false;
+  int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
+  if (tryGetEnv == JNI_EDETACHED) {
+    // Thread not attached
+    didAttachThread = true;
+    if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
+      // Failed to attach, log and return
+      wpi::outs() << "Failed to attach\n";
+      wpi::outs().flush();
+      return;
+    }
+  } else if (tryGetEnv == JNI_EVERSION) {
+    wpi::outs() << "Invalid JVM Version requested\n";
+    wpi::outs().flush();
+  }
+
+  auto [value1, value2] = ToValue12(value);
+  env->CallVoidMethod(m_call, simValueCallbackCallback, MakeJString(env, name),
+                      (jint)handle, (jboolean)readonly, (jint)value.type,
+                      value1, value2);
+
+  if (env->ExceptionCheck()) {
+    env->ExceptionDescribe();
+  }
+
+  if (didAttachThread) {
+    vm->DetachCurrentThread();
+  }
 }
 
-void CallbackJNI::SendValue(int32_t callback, ValueInfo info) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-  thr->m_valueCalls.emplace_back(thr->m_callbacks[callback], std::move(info));
-  thr->m_cond.notify_one();
+static hal::UnlimitedHandleResource<SIM_JniHandle, DeviceCallbackStore,
+                                    hal::HAL_HandleEnum::SimulationJni>*
+    deviceCallbackHandles;
+
+namespace {
+typedef int32_t (*RegisterDeviceCallbackFunc)(const char* prefix, void* param,
+                                              HALSIM_SimDeviceCallback callback,
+                                              HAL_Bool initialNotify);
+typedef void (*FreeDeviceCallbackFunc)(int32_t uid);
+}  // namespace
+
+static SIM_JniHandle AllocateDeviceCallback(
+    JNIEnv* env, const char* prefix, jobject callback, jboolean initialNotify,
+    RegisterDeviceCallbackFunc createCallback) {
+  auto callbackStore = std::make_shared<DeviceCallbackStore>();
+
+  auto handle = deviceCallbackHandles->Allocate(callbackStore);
+
+  if (handle == HAL_kInvalidHandle) {
+    return -1;
+  }
+
+  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+  callbackStore->create(env, callback);
+
+  auto callbackFunc = [](const char* name, void* param,
+                         HAL_SimDeviceHandle handle) {
+    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+    SIM_JniHandle jnihandle = static_cast<SIM_JniHandle>(handleTmp);
+    auto data = deviceCallbackHandles->Get(jnihandle);
+    if (!data) return;
+
+    data->performCallback(name, handle);
+  };
+
+  auto id =
+      createCallback(prefix, handleAsVoidPtr, callbackFunc, initialNotify);
+
+  callbackStore->setCallbackId(id);
+
+  return handle;
 }
 
-std::pair<int32_t, std::shared_ptr<CallbackStore>>
-CallbackJNI::AllocateCallback(JNIEnv* env, jobject obj) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return std::pair(0, nullptr);
-  auto store = std::make_shared<CallbackStore>(env, obj);
-  return std::pair(thr->m_callbacks.emplace_back(store) + 1, store);
+static void FreeDeviceCallback(JNIEnv* env, SIM_JniHandle handle,
+                               FreeDeviceCallbackFunc freeCallback) {
+  auto callback = deviceCallbackHandles->Free(handle);
+  freeCallback(callback->getCallbackId());
+  callback->free(env);
 }
 
-void CallbackJNI::FreeCallback(JNIEnv* env, int32_t uid) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-  if (uid <= 0 || static_cast<uint32_t>(uid) >= thr->m_callbacks.size()) return;
-  --uid;
-  auto store = std::move(thr->m_callbacks[uid]);
-  thr->m_callbacks.erase(uid);
-  store->Free(env);
+static hal::UnlimitedHandleResource<SIM_JniHandle, ValueCallbackStore,
+                                    hal::HAL_HandleEnum::SimulationJni>*
+    valueCallbackHandles;
+
+namespace {
+typedef void (*FreeValueCallbackFunc)(int32_t uid);
+}  // namespace
+
+template <typename THandle>
+static SIM_JniHandle AllocateValueCallback(
+    JNIEnv* env, THandle h, jobject callback, jboolean initialNotify,
+    int32_t (*createCallback)(THandle handle, void* param,
+                              HALSIM_SimValueCallback callback,
+                              HAL_Bool initialNotify)) {
+  auto callbackStore = std::make_shared<ValueCallbackStore>();
+
+  auto handle = valueCallbackHandles->Allocate(callbackStore);
+
+  if (handle == HAL_kInvalidHandle) {
+    return -1;
+  }
+
+  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+  callbackStore->create(env, callback);
+
+  auto callbackFunc = [](const char* name, void* param,
+                         HAL_SimValueHandle handle, HAL_Bool readonly,
+                         const HAL_Value* value) {
+    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+    SIM_JniHandle jnihandle = static_cast<SIM_JniHandle>(handleTmp);
+    auto data = valueCallbackHandles->Get(jnihandle);
+    if (!data) return;
+
+    data->performCallback(name, handle, readonly, *value);
+  };
+
+  auto id = createCallback(h, handleAsVoidPtr, callbackFunc, initialNotify);
+
+  callbackStore->setCallbackId(id);
+
+  return handle;
+}
+
+static void FreeValueCallback(JNIEnv* env, SIM_JniHandle handle,
+                              FreeValueCallbackFunc freeCallback) {
+  auto callback = valueCallbackHandles->Free(handle);
+  freeCallback(callback->getCallbackId());
+  callback->free(env);
 }
 
 namespace hal {
@@ -288,6 +325,16 @@ bool InitializeSimDeviceDataJNI(JNIEnv* env) {
       simValueCallbackCls, "callbackNative", "(Ljava/lang/String;IZIJD)V");
   if (!simValueCallbackCallback) return false;
 
+  static hal::UnlimitedHandleResource<SIM_JniHandle, DeviceCallbackStore,
+                                      hal::HAL_HandleEnum::SimulationJni>
+      cbDevice;
+  deviceCallbackHandles = &cbDevice;
+
+  static hal::UnlimitedHandleResource<SIM_JniHandle, ValueCallbackStore,
+                                      hal::HAL_HandleEnum::SimulationJni>
+      cbValue;
+  valueCallbackHandles = &cbValue;
+
   return true;
 }
 
@@ -337,18 +384,9 @@ Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_registerSimDeviceCreatedCallb
   (JNIEnv* env, jclass, jstring prefix, jobject callback,
    jboolean initialNotify)
 {
-  auto [uid, store] =
-      CallbackJNI::GetInstance().AllocateCallback(env, callback);
-  int32_t cuid = HALSIM_RegisterSimDeviceCreatedCallback(
-      JStringRef{env, prefix}.c_str(),
-      reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
-      [](const char* name, void* param, HAL_SimDeviceHandle handle) {
-        int32_t uid = reinterpret_cast<intptr_t>(param);
-        CallbackJNI::GetInstance().SendDevice(uid, DeviceInfo{name, handle});
-      },
-      initialNotify);
-  store->SetCancel([cuid] { HALSIM_CancelSimDeviceCreatedCallback(cuid); });
-  return uid;
+  return AllocateDeviceCallback(env, JStringRef{env, prefix}.c_str(), callback,
+                                initialNotify,
+                                &HALSIM_RegisterSimDeviceCreatedCallback);
 }
 
 /*
@@ -360,29 +398,22 @@ JNIEXPORT void JNICALL
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_cancelSimDeviceCreatedCallback
   (JNIEnv* env, jclass, jint uid)
 {
-  CallbackJNI::GetInstance().FreeCallback(env, uid);
+  FreeDeviceCallback(env, uid, &HALSIM_CancelSimDeviceCreatedCallback);
 }
 
 /*
  * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
  * Method:    registerSimDeviceFreedCallback
- * Signature: (Ljava/lang/String;Ljava/lang/Object;)I
+ * Signature: (Ljava/lang/String;Ljava/lang/Object;Z)I
  */
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_registerSimDeviceFreedCallback
-  (JNIEnv* env, jclass, jstring prefix, jobject callback)
+  (JNIEnv* env, jclass, jstring prefix, jobject callback,
+   jboolean initialNotify)
 {
-  auto [uid, store] =
-      CallbackJNI::GetInstance().AllocateCallback(env, callback);
-  int32_t cuid = HALSIM_RegisterSimDeviceFreedCallback(
-      JStringRef{env, prefix}.c_str(),
-      reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
-      [](const char* name, void* param, HAL_SimDeviceHandle handle) {
-        int32_t uid = reinterpret_cast<intptr_t>(param);
-        CallbackJNI::GetInstance().SendDevice(uid, DeviceInfo{name, handle});
-      });
-  store->SetCancel([cuid] { HALSIM_CancelSimDeviceFreedCallback(cuid); });
-  return uid;
+  return AllocateDeviceCallback(env, JStringRef{env, prefix}.c_str(), callback,
+                                initialNotify,
+                                &HALSIM_RegisterSimDeviceFreedCallback);
 }
 
 /*
@@ -394,7 +425,7 @@ JNIEXPORT void JNICALL
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_cancelSimDeviceFreedCallback
   (JNIEnv* env, jclass, jint uid)
 {
-  CallbackJNI::GetInstance().FreeCallback(env, uid);
+  FreeDeviceCallback(env, uid, &HALSIM_CancelSimDeviceFreedCallback);
 }
 
 /*
@@ -460,19 +491,9 @@ JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_registerSimValueCreatedCallback
   (JNIEnv* env, jclass, jint device, jobject callback, jboolean initialNotify)
 {
-  auto [uid, store] =
-      CallbackJNI::GetInstance().AllocateCallback(env, callback);
-  int32_t cuid = HALSIM_RegisterSimValueCreatedCallback(
-      device, reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
-      [](const char* name, void* param, HAL_SimValueHandle handle,
-         HAL_Bool readonly, const HAL_Value* value) {
-        int32_t uid = reinterpret_cast<intptr_t>(param);
-        CallbackJNI::GetInstance().SendValue(
-            uid, ValueInfo{name, handle, static_cast<bool>(readonly), *value});
-      },
-      initialNotify);
-  store->SetCancel([cuid] { HALSIM_CancelSimValueCreatedCallback(cuid); });
-  return uid;
+  return AllocateValueCallback(env, static_cast<HAL_SimDeviceHandle>(device),
+                               callback, initialNotify,
+                               &HALSIM_RegisterSimValueCreatedCallback);
 }
 
 /*
@@ -484,7 +505,7 @@ JNIEXPORT void JNICALL
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_cancelSimValueCreatedCallback
   (JNIEnv* env, jclass, jint uid)
 {
-  CallbackJNI::GetInstance().FreeCallback(env, uid);
+  FreeValueCallback(env, uid, &HALSIM_CancelSimValueCreatedCallback);
 }
 
 /*
@@ -496,19 +517,9 @@ JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_registerSimValueChangedCallback
   (JNIEnv* env, jclass, jint handle, jobject callback, jboolean initialNotify)
 {
-  auto [uid, store] =
-      CallbackJNI::GetInstance().AllocateCallback(env, callback);
-  int32_t cuid = HALSIM_RegisterSimValueChangedCallback(
-      handle, reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
-      [](const char* name, void* param, HAL_SimValueHandle handle,
-         HAL_Bool readonly, const HAL_Value* value) {
-        int32_t uid = reinterpret_cast<intptr_t>(param);
-        CallbackJNI::GetInstance().SendValue(
-            uid, ValueInfo{name, handle, static_cast<bool>(readonly), *value});
-      },
-      initialNotify);
-  store->SetCancel([cuid] { HALSIM_CancelSimValueChangedCallback(cuid); });
-  return uid;
+  return AllocateValueCallback(env, static_cast<HAL_SimValueHandle>(handle),
+                               callback, initialNotify,
+                               &HALSIM_RegisterSimValueChangedCallback);
 }
 
 /*
@@ -520,7 +531,7 @@ JNIEXPORT void JNICALL
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_cancelSimValueChangedCallback
   (JNIEnv* env, jclass, jint uid)
 {
-  CallbackJNI::GetInstance().FreeCallback(env, uid);
+  FreeValueCallback(env, uid, &HALSIM_CancelSimValueChangedCallback);
 }
 
 /*

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

@@ -31,8 +31,9 @@ int32_t HALSIM_RegisterSimDeviceCreatedCallback(
 
 void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid);
 
-int32_t HALSIM_RegisterSimDeviceFreedCallback(
-    const char* prefix, void* param, HALSIM_SimDeviceCallback callback);
+int32_t HALSIM_RegisterSimDeviceFreedCallback(const char* prefix, void* param,
+                                              HALSIM_SimDeviceCallback callback,
+                                              HAL_Bool initialNotify);
 
 void HALSIM_CancelSimDeviceFreedCallback(int32_t uid);
 

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

@@ -21,6 +21,7 @@
 #include <wpi/raw_ostream.h>
 
 #include "HALInitializer.h"
+#include "hal/cpp/fpga_clock.h"
 #include "hal/simulation/MockHooks.h"
 #include "mockdata/DriverStationDataInternal.h"
 
@@ -66,18 +67,16 @@ int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
   static constexpr int KEEP_MSGS = 5;
   std::scoped_lock lock(msgMutex);
   static std::string prevMsg[KEEP_MSGS];
-  static std::chrono::time_point<std::chrono::steady_clock>
-      prevMsgTime[KEEP_MSGS];
+  static fpga_clock::time_point prevMsgTime[KEEP_MSGS];
   static bool initialized = false;
   if (!initialized) {
     for (int i = 0; i < KEEP_MSGS; i++) {
-      prevMsgTime[i] =
-          std::chrono::steady_clock::now() - std::chrono::seconds(2);
+      prevMsgTime[i] = fpga_clock::now() - std::chrono::seconds(2);
     }
     initialized = true;
   }
 
-  auto curTime = std::chrono::steady_clock::now();
+  auto curTime = fpga_clock::now();
   int i;
   for (i = 0; i < KEEP_MSGS; ++i) {
     if (prevMsg[i] == details) break;

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

@@ -204,8 +204,10 @@ int32_t SimDeviceData::RegisterDeviceCreatedCallback(
 
   // initial notifications
   if (initialNotify) {
-    for (auto&& device : m_devices)
-      callback(device->name.c_str(), param, device->handle);
+    for (auto&& device : m_devices) {
+      if (wpi::StringRef{device->name}.startswith(prefix))
+        callback(device->name.c_str(), param, device->handle);
+    }
   }
 
   return index;
@@ -383,8 +385,9 @@ void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid) {
   SimSimDeviceData->CancelDeviceCreatedCallback(uid);
 }
 
-int32_t HALSIM_RegisterSimDeviceFreedCallback(
-    const char* prefix, void* param, HALSIM_SimDeviceCallback callback) {
+int32_t HALSIM_RegisterSimDeviceFreedCallback(const char* prefix, void* param,
+                                              HALSIM_SimDeviceCallback callback,
+                                              HAL_Bool initialNotify) {
   return SimSimDeviceData->RegisterDeviceFreedCallback(prefix, param, callback);
 }
 

imgui/CMakeLists.txt

@@ -40,6 +40,7 @@ file(GENERATE OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/stb_image.cpp
 # Add imgui directly to our build.
 set(SAVE_BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS})
 set(BUILD_SHARED_LIBS OFF)
+set(GLFW_INSTALL OFF)
 add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/glfw-src
                  ${CMAKE_CURRENT_BINARY_DIR}/glfw-build
                  EXCLUDE_FROM_ALL)
@@ -56,7 +57,7 @@ file(GLOB implot_sources ${implot_srcdir}/*.cpp)
 add_library(imgui STATIC ${imgui_sources} ${implot_sources} ${imgui_srcdir}/examples/imgui_impl_glfw.cpp ${imgui_srcdir}/examples/imgui_impl_opengl3.cpp ${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.cpp ${CMAKE_CURRENT_BINARY_DIR}/stb_image.cpp)
 target_compile_definitions(imgui PUBLIC IMGUI_IMPL_OPENGL_LOADER_GL3W)
 if (MSVC)
-    target_sources(imgui PRIVATE ${imgui_srcdir}/examples/imgui_impl_directx11.cpp)
+    target_sources(imgui PRIVATE ${imgui_srcdir}/examples/imgui_impl_dx11.cpp)
 else()
     if (APPLE)
         target_compile_options(imgui PRIVATE -fobjc-arc)

ntcore/ntcore-config.cmake.in

@@ -2,4 +2,5 @@ include(CMakeFindDependencyMacro)
 @FILENAME_DEP_REPLACE@
 @WPIUTIL_DEP_REPLACE@
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/ntcore.cmake)

shared/config.gradle

@@ -9,7 +9,7 @@ nativeUtils {
     configureDependencies {
       wpiVersion = "-1"
       niLibVersion = "2020.10.1"
-      opencvVersion = "3.4.7-4"
+      opencvVersion = "3.4.7-5"
       googleTestVersion = "1.9.0-5-437e100-1"
       imguiVersion = "1.76-9"
       wpimathVersion = "-1"

shared/opencv.gradle

@@ -1,4 +1,4 @@
-def opencvVersion = '3.4.7-3'
+def opencvVersion = '3.4.7-5'
 
 if (project.hasProperty('useCpp') && project.useCpp) {
     model {
@@ -22,12 +22,12 @@ if (project.hasProperty('useCpp') && project.useCpp) {
 
 if (project.hasProperty('useJava') && project.useJava) {
     dependencies {
-        implementation "edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:${opencvVersion}"
+        implementation "edu.wpi.first.thirdparty.frc2021.opencv:opencv-java:${opencvVersion}"
         if (!project.hasProperty('skipDev') || !project.skipDev) {
-            devImplementation "edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:${opencvVersion}"
+            devImplementation "edu.wpi.first.thirdparty.frc2021.opencv:opencv-java:${opencvVersion}"
         }
         if (project.hasProperty('useDocumentation') && project.useDocumentation) {
-            javaSource "edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:${opencvVersion}:sources"
+            javaSource "edu.wpi.first.thirdparty.frc2021.opencv:opencv-java:${opencvVersion}:sources"
         }
     }
 }

simulation/frc_gazebo_plugins/build.gradle

@@ -21,16 +21,14 @@ try {
     gazebo_linker_args = "pkg-config --libs gazebo protobuf".execute().text.split()
 } catch(Exception ex) { }
 
-if (!gazebo_version?.trim()) {
-    println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
-    if (project.hasProperty("makeSim")) {
-        /* Force the build even though we did not find protobuf. */
+if (project.hasProperty("makeSim")) {
+    if (!gazebo_version?.trim()) {
+        println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
         println "makeSim set. Forcing build - failure likely."
     }
-    else {
-        ext.skip_frc_plugins = true
-        println "Skipping FRC Plugins."
-    }
+} else {
+    ext.skip_frc_plugins = true
+    println "Skipping FRC Plugins."
 }
 
 evaluationDependsOn(":simulation:gz_msgs")

simulation/gz_msgs/build.gradle

@@ -26,17 +26,14 @@ try {
 } catch(Exception ex) {
 }
 
-if (!protobuf_version?.trim()) {
-    println "Protobuf is not available. (pkg-config --modversion protobuf failed)"
-    protobuf_version = "+"
-    if (project.hasProperty("makeSim")) {
-        /* Force the build even though we did not find protobuf. */
+if (project.hasProperty("makeSim")) {
+    if (!protobuf_version?.trim()) {
+        println "Protobuf is not available. (pkg-config --modversion protobuf failed)"
         println "makeSim set. Forcing build - failure likely."
     }
-    else {
-        ext.skip_gz_msgs = true
-        println "Skipping gz_msgs."
-    }
+} else {
+    ext.skip_gz_msgs = true
+    println "Skipping gz msgs."
 }
 
 tasks.whenTaskAdded { task ->
@@ -88,6 +85,11 @@ model {
             binaries {
                 all {
                     cppCompiler.args "-fPIC"
+
+                    // Disable -Wzero-length-array on Clang
+                    if (it.targetPlatform.operatingSystem.isMacOsX()) {
+                        it.cppCompiler.args.add('-Wno-error=zero-length-array')
+                    }
                 }
             }
         }

simulation/halsim_ds_socket/CMakeLists.txt

@@ -4,7 +4,7 @@ include(CompileWarnings)
 
 file(GLOB halsim_ds_socket_src src/main/native/cpp/*.cpp)
 
-add_library(halsim_ds_socket MODULE ${halsim_ds_socket_src})
+add_library(halsim_ds_socket SHARED ${halsim_ds_socket_src})
 wpilib_target_warnings(halsim_ds_socket)
 set_target_properties(halsim_ds_socket PROPERTIES DEBUG_POSTFIX "d")
 target_link_libraries(halsim_ds_socket PUBLIC hal)

simulation/halsim_gazebo/build.gradle

@@ -19,16 +19,14 @@ try {
     gazebo_linker_args = "pkg-config --libs gazebo protobuf".execute().text.split()
 } catch(Exception ex) { }
 
-if (!gazebo_version?.trim()) {
-    println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
-    if (project.hasProperty("makeSim")) {
-        /* Force the build even though we did not find protobuf. */
+if (project.hasProperty("makeSim")) {
+    if (!gazebo_version?.trim()) {
+        println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
         println "makeSim set. Forcing build - failure likely."
     }
-    else {
-        ext.skip_frc_plugins = true
-        println "Skipping FRC Plugins."
-    }
+} else {
+    ext.skip_frc_plugins = true
+    println "Skipping FRC Plugins."
 }
 
 evaluationDependsOn(":simulation:gz_msgs")

simulation/halsim_gui/CMakeLists.txt

@@ -5,7 +5,7 @@ include(LinkMacOSGUI)
 
 file(GLOB halsim_gui_src src/main/native/cpp/*.cpp)
 
-add_library(halsim_gui MODULE ${halsim_gui_src})
+add_library(halsim_gui SHARED ${halsim_gui_src})
 wpilib_target_warnings(halsim_gui)
 set_target_properties(halsim_gui PROPERTIES DEBUG_POSTFIX "d")
 

simulation/halsim_ws_client/CMakeLists.txt

@@ -4,7 +4,7 @@ include(CompileWarnings)
 
 file(GLOB halsim_ws_client_src src/main/native/cpp/*.cpp)
 
-add_library(halsim_ws_client MODULE ${halsim_ws_client_src})
+add_library(halsim_ws_client SHARED ${halsim_ws_client_src})
 wpilib_target_warnings(halsim_ws_client)
 set_target_properties(halsim_ws_client PROPERTIES DEBUG_POSTFIX "d")
 target_link_libraries(halsim_ws_client PUBLIC hal halsim_ws_core)

simulation/halsim_ws_core/src/main/native/cpp/WSProvider_SimDevice.cpp

@@ -170,7 +170,7 @@ void HALSimWSProviderSimDevices::Initialize(wpi::uv::Loop& loop) {
   m_deviceCreatedCbKey = HALSIM_RegisterSimDeviceCreatedCallback(
       "", this, HALSimWSProviderSimDevices::DeviceCreatedCallbackStatic, 1);
   m_deviceFreedCbKey = HALSIM_RegisterSimDeviceFreedCallback(
-      "", this, HALSimWSProviderSimDevices::DeviceFreedCallbackStatic);
+      "", this, HALSimWSProviderSimDevices::DeviceFreedCallbackStatic, false);
 
   m_exec = UvExecFn::Create(loop, [](auto out, LoopFn func) {
     func();

simulation/halsim_ws_server/CMakeLists.txt

@@ -4,7 +4,7 @@ include(CompileWarnings)
 
 file(GLOB halsim_ws_server_src src/main/native/cpp/*.cpp)
 
-add_library(halsim_ws_server MODULE ${halsim_ws_server_src})
+add_library(halsim_ws_server SHARED ${halsim_ws_server_src})
 wpilib_target_warnings(halsim_ws_server)
 set_target_properties(halsim_ws_server PROPERTIES DEBUG_POSTFIX "d")
 target_link_libraries(halsim_ws_server PUBLIC hal halsim_ws_core)

wpigui/src/main/native/cpp/wpigui.cpp

@@ -36,6 +36,7 @@ static void WindowSizeCallback(GLFWwindow* window, int width, int height) {
     gContext->width = width;
     gContext->height = height;
   }
+  PlatformRenderFrame();
 }
 
 static void FramebufferSizeCallback(GLFWwindow* window, int width, int height) {

wpigui/src/main/native/directx11/wpigui_directx11.cpp

@@ -19,6 +19,8 @@
 #include "wpigui.h"
 #include "wpigui_internal.h"
 
+#pragma comment(lib, "d3d11.lib")
+
 using namespace wpi::gui;
 
 namespace {

wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp

@@ -44,5 +44,7 @@ double AnalogPotentiometer::Get() const {
 double AnalogPotentiometer::PIDGet() { return Get(); }
 
 void AnalogPotentiometer::InitSendable(SendableBuilder& builder) {
-  m_analog_input->InitSendable(builder);
+  builder.SetSmartDashboardType("Analog Input");
+  builder.AddDoubleProperty(
+      "Value", [=]() { return Get(); }, nullptr);
 }

wpilibc/src/main/native/cpp/DriverStation.cpp

@@ -8,6 +8,8 @@
 #include "frc/DriverStation.h"
 
 #include <chrono>
+#include <string>
+#include <type_traits>
 
 #include <hal/DriverStation.h>
 #include <hal/HALBase.h>
@@ -23,41 +25,71 @@
 #include "frc/WPIErrors.h"
 
 namespace frc {
+// A simple class which caches the previous value written to an NT entry
+// Used to prevent redundant, repeated writes of the same value
+template <class T>
+class MatchDataSenderEntry {
+ public:
+  MatchDataSenderEntry(const std::shared_ptr<nt::NetworkTable>& table,
+                       const wpi::Twine& key, const T& initialVal) {
+    static_assert(std::is_convertible<decltype(initialVal), bool>() ||
+                      std::is_convertible<decltype(initialVal), double>() ||
+                      std::is_convertible<decltype(initialVal), wpi::Twine>(),
+                  "Invalid type for MatchDataSenderEntry - must be convertable "
+                  "to bool, double or wpi::Twine");
+
+    ntEntry = table->GetEntry(key);
+    if constexpr (std::is_convertible<decltype(initialVal), bool>()) {
+      ntEntry.ForceSetBoolean(initialVal);
+    } else if constexpr (std::is_convertible<decltype(initialVal), double>()) {
+      ntEntry.ForceSetDouble(initialVal);
+    } else if constexpr (std::is_convertible<decltype(initialVal),
+                                             wpi::Twine>()) {
+      ntEntry.ForceSetString(initialVal);
+    }
+    prevVal = initialVal;
+  }
+
+  void Set(const T& val) {
+    if (val != prevVal) {
+      SetValue(val);
+      prevVal = val;
+    }
+  }
+
+ private:
+  nt::NetworkTableEntry ntEntry;
+  T prevVal;
+
+  void SetValue(bool val) { ntEntry.SetBoolean(val); }
+  void SetValue(double val) { ntEntry.SetDouble(val); }
+  void SetValue(const wpi::Twine& val) { ntEntry.SetString(val); }
+};
 
 class MatchDataSender {
  public:
   std::shared_ptr<nt::NetworkTable> table;
-  nt::NetworkTableEntry typeMetadata;
-  nt::NetworkTableEntry gameSpecificMessage;
-  nt::NetworkTableEntry eventName;
-  nt::NetworkTableEntry matchNumber;
-  nt::NetworkTableEntry replayNumber;
-  nt::NetworkTableEntry matchType;
-  nt::NetworkTableEntry alliance;
-  nt::NetworkTableEntry station;
-  nt::NetworkTableEntry controlWord;
+  MatchDataSenderEntry<std::string> typeMetaData;
+  MatchDataSenderEntry<std::string> gameSpecificMessage;
+  MatchDataSenderEntry<std::string> eventName;
+  MatchDataSenderEntry<double> matchNumber;
+  MatchDataSenderEntry<double> replayNumber;
+  MatchDataSenderEntry<double> matchType;
+  MatchDataSenderEntry<bool> alliance;
+  MatchDataSenderEntry<double> station;
+  MatchDataSenderEntry<double> controlWord;
 
-  MatchDataSender() {
-    table = nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo");
-    typeMetadata = table->GetEntry(".type");
-    typeMetadata.ForceSetString("FMSInfo");
-    gameSpecificMessage = table->GetEntry("GameSpecificMessage");
-    gameSpecificMessage.ForceSetString("");
-    eventName = table->GetEntry("EventName");
-    eventName.ForceSetString("");
-    matchNumber = table->GetEntry("MatchNumber");
-    matchNumber.ForceSetDouble(0);
-    replayNumber = table->GetEntry("ReplayNumber");
-    replayNumber.ForceSetDouble(0);
-    matchType = table->GetEntry("MatchType");
-    matchType.ForceSetDouble(0);
-    alliance = table->GetEntry("IsRedAlliance");
-    alliance.ForceSetBoolean(true);
-    station = table->GetEntry("StationNumber");
-    station.ForceSetDouble(1);
-    controlWord = table->GetEntry("FMSControlData");
-    controlWord.ForceSetDouble(0);
-  }
+  MatchDataSender()
+      : table(nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo")),
+        typeMetaData(table, ".type", "FMSInfo"),
+        gameSpecificMessage(table, "GameSpecificMessage", ""),
+        eventName(table, "EventName", ""),
+        matchNumber(table, "MatchNumber", 0.0),
+        replayNumber(table, "ReplayNumber", 0.0),
+        matchType(table, "MatchType", 0.0),
+        alliance(table, "IsRedAlliance", true),
+        station(table, "StationNumber", 1.0),
+        controlWord(table, "FMSControlData", 0.0) {}
 };
 }  // namespace frc
 
@@ -331,6 +363,11 @@ int DriverStation::GetJoystickAxisType(int stick, int axis) const {
   return static_cast<bool>(descriptor.axisTypes);
 }
 
+bool DriverStation::IsJoystickConnected(int stick) const {
+  return GetStickAxisCount(stick) > 0 || GetStickButtonCount(stick) > 0 ||
+         GetStickPOVCount(stick) > 0;
+}
+
 bool DriverStation::IsEnabled() const {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
@@ -538,6 +575,14 @@ void DriverStation::GetData() {
   SendMatchData();
 }
 
+void DriverStation::SilenceJoystickConnectionWarning(bool silence) {
+  m_silenceJoystickWarning = silence;
+}
+
+bool DriverStation::IsJoystickConnectionWarningSilenced() const {
+  return !IsFMSAttached() && m_silenceJoystickWarning;
+}
+
 DriverStation::DriverStation() {
   HAL_Initialize(500, 0);
   m_waitForDataCounter = 0;
@@ -565,10 +610,12 @@ void DriverStation::ReportJoystickUnpluggedError(const wpi::Twine& message) {
 }
 
 void DriverStation::ReportJoystickUnpluggedWarning(const wpi::Twine& message) {
-  double currentTime = Timer::GetFPGATimestamp();
-  if (currentTime > m_nextMessageTime) {
-    ReportWarning(message);
-    m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
+  if (IsFMSAttached() || !m_silenceJoystickWarning) {
+    double currentTime = Timer::GetFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      ReportWarning(message);
+      m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
+    }
   }
 }
 
@@ -627,20 +674,19 @@ void DriverStation::SendMatchData() {
   HAL_MatchInfo tmpDataStore;
   HAL_GetMatchInfo(&tmpDataStore);
 
-  m_matchDataSender->alliance.SetBoolean(isRedAlliance);
-  m_matchDataSender->station.SetDouble(stationNumber);
-  m_matchDataSender->eventName.SetString(tmpDataStore.eventName);
-  m_matchDataSender->gameSpecificMessage.SetString(
+  m_matchDataSender->alliance.Set(isRedAlliance);
+  m_matchDataSender->station.Set(stationNumber);
+  m_matchDataSender->eventName.Set(tmpDataStore.eventName);
+  m_matchDataSender->gameSpecificMessage.Set(
       std::string(reinterpret_cast<char*>(tmpDataStore.gameSpecificMessage),
                   tmpDataStore.gameSpecificMessageSize));
-  m_matchDataSender->matchNumber.SetDouble(tmpDataStore.matchNumber);
-  m_matchDataSender->replayNumber.SetDouble(tmpDataStore.replayNumber);
-  m_matchDataSender->matchType.SetDouble(
-      static_cast<int>(tmpDataStore.matchType));
+  m_matchDataSender->matchNumber.Set(tmpDataStore.matchNumber);
+  m_matchDataSender->replayNumber.Set(tmpDataStore.replayNumber);
+  m_matchDataSender->matchType.Set(static_cast<int>(tmpDataStore.matchType));
 
   HAL_ControlWord ctlWord;
   HAL_GetControlWord(&ctlWord);
   int32_t wordInt = 0;
   std::memcpy(&wordInt, &ctlWord, sizeof(wordInt));
-  m_matchDataSender->controlWord.SetDouble(wordInt);
+  m_matchDataSender->controlWord.Set(wordInt);
 }

wpilibc/src/main/native/cpp/GenericHID.cpp

@@ -47,6 +47,10 @@ int GenericHID::GetButtonCount() const {
   return m_ds->GetStickButtonCount(m_port);
 }
 
+bool GenericHID::IsConnected() const {
+  return m_ds->IsJoystickConnected(m_port);
+}
+
 GenericHID::HIDType GenericHID::GetType() const {
   return static_cast<HIDType>(m_ds->GetJoystickType(m_port));
 }

wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp

@@ -98,6 +98,8 @@ void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) {
   m_x(State::kX) = pose.X().to<double>();
   m_x(State::kY) = pose.Y().to<double>();
   m_x(State::kHeading) = pose.Rotation().Radians().to<double>();
+  m_x(State::kLeftPosition) = 0;
+  m_x(State::kRightPosition) = 0;
 }
 
 Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::Dynamics(

wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp

@@ -39,10 +39,12 @@ std::unique_ptr<CallbackStore> RoboRioSim::RegisterVInVoltageCallback(
   return store;
 }
 
-double RoboRioSim::GetVInVoltage() { return HALSIM_GetRoboRioVInVoltage(); }
+units::volt_t RoboRioSim::GetVInVoltage() {
+  return units::volt_t(HALSIM_GetRoboRioVInVoltage());
+}
 
-void RoboRioSim::SetVInVoltage(double vInVoltage) {
-  HALSIM_SetRoboRioVInVoltage(vInVoltage);
+void RoboRioSim::SetVInVoltage(units::volt_t vInVoltage) {
+  HALSIM_SetRoboRioVInVoltage(vInVoltage.to<double>());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterVInCurrentCallback(
@@ -54,10 +56,12 @@ std::unique_ptr<CallbackStore> RoboRioSim::RegisterVInCurrentCallback(
   return store;
 }
 
-double RoboRioSim::GetVInCurrent() { return HALSIM_GetRoboRioVInCurrent(); }
+units::ampere_t RoboRioSim::GetVInCurrent() {
+  return units::ampere_t(HALSIM_GetRoboRioVInCurrent());
+}
 
-void RoboRioSim::SetVInCurrent(double vInCurrent) {
-  HALSIM_SetRoboRioVInCurrent(vInCurrent);
+void RoboRioSim::SetVInCurrent(units::ampere_t vInCurrent) {
+  HALSIM_SetRoboRioVInCurrent(vInCurrent.to<double>());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserVoltage6VCallback(
@@ -69,12 +73,12 @@ std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserVoltage6VCallback(
   return store;
 }
 
-double RoboRioSim::GetUserVoltage6V() {
-  return HALSIM_GetRoboRioUserVoltage6V();
+units::volt_t RoboRioSim::GetUserVoltage6V() {
+  return units::volt_t(HALSIM_GetRoboRioUserVoltage6V());
 }
 
-void RoboRioSim::SetUserVoltage6V(double userVoltage6V) {
-  HALSIM_SetRoboRioUserVoltage6V(userVoltage6V);
+void RoboRioSim::SetUserVoltage6V(units::volt_t userVoltage6V) {
+  HALSIM_SetRoboRioUserVoltage6V(userVoltage6V.to<double>());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent6VCallback(
@@ -86,12 +90,12 @@ std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent6VCallback(
   return store;
 }
 
-double RoboRioSim::GetUserCurrent6V() {
-  return HALSIM_GetRoboRioUserCurrent6V();
+units::ampere_t RoboRioSim::GetUserCurrent6V() {
+  return units::ampere_t(HALSIM_GetRoboRioUserCurrent6V());
 }
 
-void RoboRioSim::SetUserCurrent6V(double userCurrent6V) {
-  HALSIM_SetRoboRioUserCurrent6V(userCurrent6V);
+void RoboRioSim::SetUserCurrent6V(units::ampere_t userCurrent6V) {
+  HALSIM_SetRoboRioUserCurrent6V(userCurrent6V.to<double>());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserActive6VCallback(
@@ -118,12 +122,12 @@ std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserVoltage5VCallback(
   return store;
 }
 
-double RoboRioSim::GetUserVoltage5V() {
-  return HALSIM_GetRoboRioUserVoltage5V();
+units::volt_t RoboRioSim::GetUserVoltage5V() {
+  return units::volt_t(HALSIM_GetRoboRioUserVoltage5V());
 }
 
-void RoboRioSim::SetUserVoltage5V(double userVoltage5V) {
-  HALSIM_SetRoboRioUserVoltage5V(userVoltage5V);
+void RoboRioSim::SetUserVoltage5V(units::volt_t userVoltage5V) {
+  HALSIM_SetRoboRioUserVoltage5V(userVoltage5V.to<double>());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent5VCallback(
@@ -135,12 +139,12 @@ std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent5VCallback(
   return store;
 }
 
-double RoboRioSim::GetUserCurrent5V() {
-  return HALSIM_GetRoboRioUserCurrent5V();
+units::ampere_t RoboRioSim::GetUserCurrent5V() {
+  return units::ampere_t(HALSIM_GetRoboRioUserCurrent5V());
 }
 
-void RoboRioSim::SetUserCurrent5V(double userCurrent5V) {
-  HALSIM_SetRoboRioUserCurrent5V(userCurrent5V);
+void RoboRioSim::SetUserCurrent5V(units::ampere_t userCurrent5V) {
+  HALSIM_SetRoboRioUserCurrent5V(userCurrent5V.to<double>());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserActive5VCallback(
@@ -167,12 +171,12 @@ std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserVoltage3V3Callback(
   return store;
 }
 
-double RoboRioSim::GetUserVoltage3V3() {
-  return HALSIM_GetRoboRioUserVoltage3V3();
+units::volt_t RoboRioSim::GetUserVoltage3V3() {
+  return units::volt_t(HALSIM_GetRoboRioUserVoltage3V3());
 }
 
-void RoboRioSim::SetUserVoltage3V3(double userVoltage3V3) {
-  HALSIM_SetRoboRioUserVoltage3V3(userVoltage3V3);
+void RoboRioSim::SetUserVoltage3V3(units::volt_t userVoltage3V3) {
+  HALSIM_SetRoboRioUserVoltage3V3(userVoltage3V3.to<double>());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent3V3Callback(
@@ -184,12 +188,12 @@ std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent3V3Callback(
   return store;
 }
 
-double RoboRioSim::GetUserCurrent3V3() {
-  return HALSIM_GetRoboRioUserCurrent3V3();
+units::ampere_t RoboRioSim::GetUserCurrent3V3() {
+  return units::ampere_t(HALSIM_GetRoboRioUserCurrent3V3());
 }
 
-void RoboRioSim::SetUserCurrent3V3(double userCurrent3V3) {
-  HALSIM_SetRoboRioUserCurrent3V3(userCurrent3V3);
+void RoboRioSim::SetUserCurrent3V3(units::ampere_t userCurrent3V3) {
+  HALSIM_SetRoboRioUserCurrent3V3(userCurrent3V3.to<double>());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserActive3V3Callback(

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -163,7 +163,9 @@ bool SendableRegistry::Remove(Sendable* sendable) {
 void SendableRegistry::Move(Sendable* to, Sendable* from) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(from);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   UID compUid = it->getSecond();
   m_impl->componentMap.erase(it);
   m_impl->componentMap[to] = compUid;
@@ -188,14 +190,18 @@ bool SendableRegistry::Contains(const Sendable* sendable) const {
 std::string SendableRegistry::GetName(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return std::string{};
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return std::string{};
   return m_impl->components[it->getSecond() - 1]->name;
 }
 
 void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->name = name.str();
 }
 
@@ -203,7 +209,9 @@ void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
                                int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->SetName(moduleType, channel);
 }
 
@@ -211,7 +219,9 @@ void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
                                int moduleNumber, int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->SetName(moduleType, moduleNumber,
                                                    channel);
 }
@@ -220,7 +230,9 @@ void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& subsystem,
                                const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   auto& comp = *m_impl->components[it->getSecond() - 1];
   comp.name = name.str();
   comp.subsystem = subsystem.str();
@@ -229,7 +241,9 @@ void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& subsystem,
 std::string SendableRegistry::GetSubsystem(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return std::string{};
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return std::string{};
   return m_impl->components[it->getSecond() - 1]->subsystem;
 }
 
@@ -237,7 +251,9 @@ void SendableRegistry::SetSubsystem(Sendable* sendable,
                                     const wpi::Twine& subsystem) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->subsystem = subsystem.str();
 }
 
@@ -251,7 +267,9 @@ std::shared_ptr<void> SendableRegistry::SetData(Sendable* sendable, int handle,
   assert(handle >= 0);
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return nullptr;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return nullptr;
   auto& comp = *m_impl->components[it->getSecond() - 1];
   std::shared_ptr<void> rv;
   if (static_cast<size_t>(handle) < comp.data.size())
@@ -267,7 +285,9 @@ std::shared_ptr<void> SendableRegistry::GetData(Sendable* sendable,
   assert(handle >= 0);
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return nullptr;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return nullptr;
   auto& comp = *m_impl->components[it->getSecond() - 1];
   if (static_cast<size_t>(handle) >= comp.data.size()) return nullptr;
   return comp.data[handle];
@@ -276,14 +296,18 @@ std::shared_ptr<void> SendableRegistry::GetData(Sendable* sendable,
 void SendableRegistry::EnableLiveWindow(Sendable* sendable) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->liveWindow = true;
 }
 
 void SendableRegistry::DisableLiveWindow(Sendable* sendable) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->liveWindow = false;
 }
 
@@ -298,13 +322,17 @@ SendableRegistry::UID SendableRegistry::GetUniqueId(Sendable* sendable) {
 Sendable* SendableRegistry::GetSendable(UID uid) {
   if (uid == 0) return nullptr;
   std::scoped_lock lock(m_impl->mutex);
+  if ((uid - 1) >= m_impl->components.size() || !m_impl->components[uid - 1])
+    return nullptr;
   return m_impl->components[uid - 1]->sendable;
 }
 
 void SendableRegistry::Publish(UID sendableUid,
                                std::shared_ptr<NetworkTable> table) {
   std::scoped_lock lock(m_impl->mutex);
-  if (sendableUid == 0) return;
+  if (sendableUid == 0 || (sendableUid - 1) >= m_impl->components.size() ||
+      !m_impl->components[sendableUid - 1])
+    return;
   auto& comp = *m_impl->components[sendableUid - 1];
   comp.builder = SendableBuilderImpl{};  // clear any current builder
   comp.builder.SetTable(table);
@@ -316,6 +344,9 @@ void SendableRegistry::Publish(UID sendableUid,
 void SendableRegistry::Update(UID sendableUid) {
   if (sendableUid == 0) return;
   std::scoped_lock lock(m_impl->mutex);
+  if ((sendableUid - 1) >= m_impl->components.size() ||
+      !m_impl->components[sendableUid - 1])
+    return;
   m_impl->components[sendableUid - 1]->builder.UpdateTable();
 }
 
@@ -327,7 +358,7 @@ void SendableRegistry::ForeachLiveWindow(
   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 (comp && comp->sendable && comp->liveWindow) {
       if (static_cast<size_t>(dataHandle) >= comp->data.size())
         comp->data.resize(dataHandle + 1);
       CallbackData cbdata{comp->sendable,         comp->name,

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

@@ -181,6 +181,17 @@ class DriverStation : public ErrorBase {
    */
   int GetJoystickAxisType(int stick, int axis) const;
 
+  /**
+   * Returns if a joystick is connected to the Driver Station.
+   *
+   * This makes a best effort guess by looking at the reported number of axis,
+   * buttons, and POVs attached.
+   *
+   * @param stick The joystick port number
+   * @return true if a joystick is connected
+   */
+  bool IsJoystickConnected(int stick) const;
+
   /**
    * Check if the DS has enabled the robot.
    *
@@ -420,6 +431,23 @@ class DriverStation : public ErrorBase {
    */
   void WakeupWaitForData();
 
+  /**
+   * Allows the user to specify whether they want joystick connection warnings
+   * to be printed to the console. This setting is ignored when the FMS is
+   * connected -- warnings will always be on in that scenario.
+   *
+   * @param silence Whether warning messages should be silenced.
+   */
+  void SilenceJoystickConnectionWarning(bool silence);
+
+  /**
+   * Returns whether joystick connection warnings are silenced. This will
+   * always return false when connected to the FMS.
+   *
+   * @return Whether joystick connection warnings are silenced.
+   */
+  bool IsJoystickConnectionWarningSilenced() const;
+
  protected:
   /**
    * Copy data from the DS task for the user.
@@ -471,6 +499,8 @@ class DriverStation : public ErrorBase {
   wpi::condition_variable m_waitForDataCond;
   int m_waitForDataCounter;
 
+  bool m_silenceJoystickWarning = false;
+
   // Robot state status variables
   bool m_userInDisabled = false;
   bool m_userInAutonomous = false;

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

@@ -136,6 +136,13 @@ class GenericHID : public ErrorBase {
    */
   int GetButtonCount() const;
 
+  /**
+   * Get if the HID is connected.
+   *
+   * @return true if the HID is connected
+   */
+  bool IsConnected() const;
+
   /**
    * Get the type of the HID.
    *

wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h

@@ -9,6 +9,9 @@
 
 #include <memory>
 
+#include <units/current.h>
+#include <units/voltage.h>
+
 #include "frc/simulation/CallbackStore.h"
 
 namespace frc {
@@ -29,30 +32,30 @@ class RoboRioSim {
   static std::unique_ptr<CallbackStore> RegisterVInVoltageCallback(
       NotifyCallback callback, bool initialNotify);
 
-  static double GetVInVoltage();
+  static units::volt_t GetVInVoltage();
 
-  static void SetVInVoltage(double vInVoltage);
+  static void SetVInVoltage(units::volt_t vInVoltage);
 
   static std::unique_ptr<CallbackStore> RegisterVInCurrentCallback(
       NotifyCallback callback, bool initialNotify);
 
-  static double GetVInCurrent();
+  static units::ampere_t GetVInCurrent();
 
-  static void SetVInCurrent(double vInCurrent);
+  static void SetVInCurrent(units::ampere_t vInCurrent);
 
   static std::unique_ptr<CallbackStore> RegisterUserVoltage6VCallback(
       NotifyCallback callback, bool initialNotify);
 
-  static double GetUserVoltage6V();
+  static units::volt_t GetUserVoltage6V();
 
-  static void SetUserVoltage6V(double userVoltage6V);
+  static void SetUserVoltage6V(units::volt_t userVoltage6V);
 
   static std::unique_ptr<CallbackStore> RegisterUserCurrent6VCallback(
       NotifyCallback callback, bool initialNotify);
 
-  static double GetUserCurrent6V();
+  static units::ampere_t GetUserCurrent6V();
 
-  static void SetUserCurrent6V(double userCurrent6V);
+  static void SetUserCurrent6V(units::ampere_t userCurrent6V);
 
   static std::unique_ptr<CallbackStore> RegisterUserActive6VCallback(
       NotifyCallback callback, bool initialNotify);
@@ -64,16 +67,16 @@ class RoboRioSim {
   static std::unique_ptr<CallbackStore> RegisterUserVoltage5VCallback(
       NotifyCallback callback, bool initialNotify);
 
-  static double GetUserVoltage5V();
+  static units::volt_t GetUserVoltage5V();
 
-  static void SetUserVoltage5V(double userVoltage5V);
+  static void SetUserVoltage5V(units::volt_t userVoltage5V);
 
   static std::unique_ptr<CallbackStore> RegisterUserCurrent5VCallback(
       NotifyCallback callback, bool initialNotify);
 
-  static double GetUserCurrent5V();
+  static units::ampere_t GetUserCurrent5V();
 
-  static void SetUserCurrent5V(double userCurrent5V);
+  static void SetUserCurrent5V(units::ampere_t userCurrent5V);
 
   static std::unique_ptr<CallbackStore> RegisterUserActive5VCallback(
       NotifyCallback callback, bool initialNotify);
@@ -85,16 +88,16 @@ class RoboRioSim {
   static std::unique_ptr<CallbackStore> RegisterUserVoltage3V3Callback(
       NotifyCallback callback, bool initialNotify);
 
-  static double GetUserVoltage3V3();
+  static units::volt_t GetUserVoltage3V3();
 
-  static void SetUserVoltage3V3(double userVoltage3V3);
+  static void SetUserVoltage3V3(units::volt_t userVoltage3V3);
 
   static std::unique_ptr<CallbackStore> RegisterUserCurrent3V3Callback(
       NotifyCallback callback, bool initialNotify);
 
-  static double GetUserCurrent3V3();
+  static units::ampere_t GetUserCurrent3V3();
 
-  static void SetUserCurrent3V3(double userCurrent3V3);
+  static void SetUserCurrent3V3(units::ampere_t userCurrent3V3);
 
   static std::unique_ptr<CallbackStore> RegisterUserActive3V3Callback(
       NotifyCallback callback, bool initialNotify);

wpilibc/src/test/native/cpp/DriverStationTest.cpp

@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <string>
+#include <tuple>
+
+#include "frc/DriverStation.h"
+#include "frc/Joystick.h"
+#include "frc/simulation/DriverStationSim.h"
+#include "frc/simulation/SimHooks.h"
+#include "gtest/gtest.h"
+
+class IsJoystickConnectedParametersTests
+    : public ::testing::TestWithParam<std::tuple<int, int, int, bool>> {};
+
+TEST_P(IsJoystickConnectedParametersTests, IsJoystickConnected) {
+  frc::sim::DriverStationSim::SetJoystickAxisCount(1, std::get<0>(GetParam()));
+  frc::sim::DriverStationSim::SetJoystickButtonCount(1,
+                                                     std::get<1>(GetParam()));
+  frc::sim::DriverStationSim::SetJoystickPOVCount(1, std::get<2>(GetParam()));
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  ASSERT_EQ(std::get<3>(GetParam()),
+            frc::DriverStation::GetInstance().IsJoystickConnected(1));
+}
+
+INSTANTIATE_TEST_SUITE_P(IsConnectedTests, IsJoystickConnectedParametersTests,
+                         ::testing::Values(std::make_tuple(0, 0, 0, false),
+                                           std::make_tuple(1, 0, 0, true),
+                                           std::make_tuple(0, 1, 0, true),
+                                           std::make_tuple(0, 0, 1, true),
+                                           std::make_tuple(1, 1, 1, true),
+                                           std::make_tuple(4, 10, 1, true)));
+class JoystickConnectionWarningTests
+    : public ::testing::TestWithParam<
+          std::tuple<bool, bool, bool, std::string>> {};
+
+TEST_P(JoystickConnectionWarningTests, JoystickConnectionWarnings) {
+  // Capture all output to stderr.
+  ::testing::internal::CaptureStderr();
+
+  // Set FMS and Silence settings
+  frc::sim::DriverStationSim::SetFmsAttached(std::get<0>(GetParam()));
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::DriverStation::GetInstance().SilenceJoystickConnectionWarning(
+      std::get<1>(GetParam()));
+
+  // Create joystick and attempt to retrieve button.
+  frc::Joystick joystick(0);
+  joystick.GetRawButton(1);
+
+  frc::sim::StepTiming(1_s);
+  EXPECT_EQ(
+      frc::DriverStation::GetInstance().IsJoystickConnectionWarningSilenced(),
+      std::get<2>(GetParam()));
+  EXPECT_EQ(::testing::internal::GetCapturedStderr(), std::get<3>(GetParam()));
+}
+
+INSTANTIATE_TEST_SUITE_P(
+    DriverStation, JoystickConnectionWarningTests,
+    ::testing::Values(std::make_tuple(false, true, true, ""),
+                      std::make_tuple(false, false, false,
+                                      "Joystick Button missing, check if all "
+                                      "controllers are plugged in\n"),
+                      std::make_tuple(true, true, false,
+                                      "Joystick Button missing, check if all "
+                                      "controllers are plugged in\n"),
+                      std::make_tuple(true, false, false,
+                                      "Joystick Button missing, check if all "
+                                      "controllers are plugged in\n")));

wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp

@@ -29,7 +29,8 @@
 
 TEST(StateSpaceSimTest, TestFlywheelSim) {
   const frc::LinearSystem<1, 1, 1> plant =
-      frc::LinearSystemId::IdentifyVelocitySystem(0.02, 0.01);
+      frc::LinearSystemId::IdentifyVelocitySystem<units::radian>(
+          0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq);
   frc::sim::FlywheelSim sim{plant, frc::DCMotor::NEO(2), 1.0};
   frc2::PIDController controller{0.2, 0.0, 0.0};
   frc::SimpleMotorFeedforward<units::radian> feedforward{
@@ -46,7 +47,7 @@ TEST(StateSpaceSimTest, TestFlywheelSim) {
 
     // Then, SimulationPeriodic runs
     frc::sim::RoboRioSim::SetVInVoltage(
-        frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}).to<double>());
+        frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
     sim.SetInput(frc::MakeMatrix<1, 1>(
         motor.Get() * frc::RobotController::GetInputVoltage()));
     sim.Update(20_ms);

wpilibc/wpilibc-config.cmake.in

@@ -1,9 +1,11 @@
 include(CMakeFindDependencyMacro)
 @FILENAME_DEP_REPLACE@
 @WPIUTIL_DEP_REPLACE@
+@WPIMATH_DEP_REPLACE@
 @NTCORE_DEP_REPLACE@
 @CSCORE_DEP_REPLACE@
 @CAMERASERVER_DEP_REPLACE@
 @HAL_DEP_REPLACE@
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/wpilibc.cmake)

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

@@ -82,8 +82,7 @@ class Robot : public frc::TimedRobot {
     m_encoderSim.SetDistance(m_armSim.GetAngle().to<double>());
     // SimBattery estimates loaded battery voltages
     frc::sim::RoboRioSim::SetVInVoltage(
-        frc::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()})
-            .to<double>());
+        frc::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()}));
   }
 
   void TeleopPeriodic() {

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

@@ -1,58 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "Drivetrain.h"
-
-#include <frc/RobotController.h>
-
-void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
-  auto leftFeedforward = m_feedforward.Calculate(speeds.left);
-  auto rightFeedforward = m_feedforward.Calculate(speeds.right);
-  double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(),
-                                                    speeds.left.to<double>());
-  double rightOutput = m_rightPIDController.Calculate(
-      m_rightEncoder.GetRate(), speeds.right.to<double>());
-
-  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
-  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
-}
-
-void Drivetrain::Drive(units::meters_per_second_t xSpeed,
-                       units::radians_per_second_t rot) {
-  SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
-}
-
-void Drivetrain::UpdateOdometry() {
-  m_odometry.Update(m_gyro.GetRotation2d(),
-                    units::meter_t(m_leftEncoder.GetDistance()),
-                    units::meter_t(m_rightEncoder.GetDistance()));
-}
-
-void Drivetrain::SimulationPeriodic() {
-  // To update our simulation, we set motor voltage inputs, update the
-  // simulation, and write the simulated positions and velocities to our
-  // simulated encoder and gyro. We negate the right side so that positive
-  // voltages make the right side move forward.
-  m_drivetrainSimulator.SetInputs(units::volt_t{m_leftLeader.Get()} *
-                                      frc::RobotController::GetInputVoltage(),
-                                  units::volt_t{-m_rightLeader.Get()} *
-                                      frc::RobotController::GetInputVoltage());
-  m_drivetrainSimulator.Update(20_ms);
-
-  m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetState(
-      frc::sim::DifferentialDrivetrainSim::State::kLeftPosition));
-  m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetState(
-      frc::sim::DifferentialDrivetrainSim::State::kLeftVelocity));
-  m_rightEncoderSim.SetDistance(m_drivetrainSimulator.GetState(
-      frc::sim::DifferentialDrivetrainSim::State::kRightPosition));
-  m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetState(
-      frc::sim::DifferentialDrivetrainSim::State::kRightVelocity));
-  m_gyroSim.SetAngle(
-      -m_drivetrainSimulator.GetHeading().Degrees().to<double>());
-
-  m_fieldSim.SetRobotPose(m_odometry.GetPose());
-}

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

@@ -1,54 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <frc/SlewRateLimiter.h>
-#include <frc/TimedRobot.h>
-#include <frc/XboxController.h>
-
-#include "Drivetrain.h"
-
-class Robot : public frc::TimedRobot {
- public:
-  void AutonomousPeriodic() override {
-    TeleopPeriodic();
-    m_drive.UpdateOdometry();
-  }
-
-  void TeleopPeriodic() override {
-    // Get the x speed. We are inverting this because Xbox controllers return
-    // negative values when we push forward.
-    const auto xSpeed = -m_speedLimiter.Calculate(
-                            m_controller.GetY(frc::GenericHID::kLeftHand)) *
-                        Drivetrain::kMaxSpeed;
-
-    // Get the rate of angular rotation. We are inverting this because we want a
-    // positive value when we pull to the left (remember, CCW is positive in
-    // mathematics). Xbox controllers return positive values when you pull to
-    // the right by default.
-    auto rot = -m_rotLimiter.Calculate(
-                   m_controller.GetX(frc::GenericHID::kRightHand)) *
-               Drivetrain::kMaxAngularSpeed;
-
-    m_drive.Drive(xSpeed, rot);
-  }
-
-  void SimulationPeriodic() override { m_drive.SimulationPeriodic(); }
-
- private:
-  frc::XboxController m_controller{0};
-
-  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
-  // to 1.
-  frc::SlewRateLimiter<units::scalar> m_speedLimiter{3 / 1_s};
-  frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
-
-  Drivetrain m_drive;
-};
-
-#ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
-#endif

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

@@ -1,97 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/AnalogGyro.h>
-#include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SpeedControllerGroup.h>
-#include <frc/controller/PIDController.h>
-#include <frc/controller/SimpleMotorFeedforward.h>
-#include <frc/kinematics/DifferentialDriveKinematics.h>
-#include <frc/kinematics/DifferentialDriveOdometry.h>
-#include <frc/simulation/AnalogGyroSim.h>
-#include <frc/simulation/DifferentialDrivetrainSim.h>
-#include <frc/simulation/EncoderSim.h>
-#include <frc/simulation/Field2d.h>
-#include <frc/system/plant/LinearSystemId.h>
-#include <units/angle.h>
-#include <units/angular_velocity.h>
-#include <units/length.h>
-#include <units/velocity.h>
-#include <wpi/math>
-
-/**
- * Represents a differential drive style drivetrain.
- */
-class Drivetrain {
- public:
-  Drivetrain() {
-    m_gyro.Reset();
-    // Set the distance per pulse for the drive encoders. We can simply use the
-    // distance traveled for one rotation of the wheel divided by the encoder
-    // resolution.
-    m_leftEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
-                                      kEncoderResolution);
-    m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
-                                       kEncoderResolution);
-
-    m_leftEncoder.Reset();
-    m_rightEncoder.Reset();
-  }
-
-  static constexpr units::meters_per_second_t kMaxSpeed =
-      3.0_mps;  // 3 meters per second
-  static constexpr units::radians_per_second_t kMaxAngularSpeed{
-      wpi::math::pi};  // 1/2 rotation per second
-
-  void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
-  void Drive(units::meters_per_second_t xSpeed,
-             units::radians_per_second_t rot);
-  void UpdateOdometry();
-
-  void SimulationPeriodic();
-
- private:
-  static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
-  static constexpr double kWheelRadius = 0.0508;  // meters
-  static constexpr int kEncoderResolution = 4096;
-
-  frc::PWMVictorSPX m_leftLeader{1};
-  frc::PWMVictorSPX m_leftFollower{2};
-  frc::PWMVictorSPX m_rightLeader{3};
-  frc::PWMVictorSPX m_rightFollower{4};
-
-  frc::SpeedControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
-  frc::SpeedControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
-
-  frc::Encoder m_leftEncoder{0, 1};
-  frc::Encoder m_rightEncoder{2, 3};
-
-  frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0};
-  frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0};
-
-  frc::AnalogGyro m_gyro{0};
-
-  frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
-  frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
-
-  // Gains are for example purposes only - must be determined for your own
-  // robot!
-  frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
-
-  // Simulation classes help us simulate our robot
-  frc::sim::AnalogGyroSim m_gyroSim{m_gyro};
-  frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
-  frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
-  frc::Field2d m_fieldSim{};
-  frc::LinearSystem<2, 2, 2> m_drivetrainSystem =
-      frc::LinearSystemId::IdentifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
-  frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
-      m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in};
-};

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

@@ -81,8 +81,7 @@ class Robot : public frc::TimedRobot {
     m_encoderSim.SetDistance(m_elevatorSim.GetPosition().to<double>());
     // SimBattery estimates loaded battery voltages
     frc::sim::RoboRioSim::SetVInVoltage(
-        frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()})
-            .to<double>());
+        frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
   }
 
   void TeleopPeriodic() {

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -103,6 +103,9 @@ frc2::Command* RobotContainer::GetAutonomousCommand() {
 
       {&m_drive});
 
+  // Reset odometry to the starting pose of the trajectory.
+  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+
   // no auto
   return new frc2::SequentialCommandGroup(
       std::move(mecanumControllerCommand),

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -84,6 +84,9 @@ frc2::Command* RobotContainer::GetAutonomousCommand() {
       [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
       {&m_drive});
 
+  // Reset odometry to the starting pose of the trajectory.
+  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+
   // no auto
   return new frc2::SequentialCommandGroup(
       std::move(ramseteCommand),

wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp

@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+const frc::DifferentialDriveKinematics DriveConstants::kDriveKinematics(
+    kTrackwidth);
+
+const frc::LinearSystem<2, 2, 2> DriveConstants::kDrivetrainPlant =
+    frc::LinearSystemId::IdentifyDrivetrainSystem(kv, ka, kvAngular, kaAngular);

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

@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/simulation/BatterySim.h>
+#include <frc/simulation/RoboRioSim.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() {
+  frc2::CommandScheduler::GetInstance().CancelAll();
+}
+
+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() {}
+
+void Robot::SimulationPeriodic() {
+  // Here we calculate the battery voltage based on drawn current.
+  // As our robot draws more power from the battery its voltage drops.
+  // The estimated voltage is highly dependent on the battery's internal
+  // resistance.
+  auto current = m_container.GetRobotDrive().GetCurrentDraw();
+  auto loadedVoltage = frc::sim::BatterySim::Calculate({current});
+  frc::sim::RoboRioSim::SetVInVoltage(loadedVoltage);
+}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

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

@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc/trajectory/Trajectory.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/RamseteCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/button/JoystickButton.h>
+
+#include "Constants.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}));
+}
+
+const DriveSubsystem& RobotContainer::GetRobotDrive() const { return 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);
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+  // Create a voltage constraint to ensure we don't accelerate too fast
+  frc::DifferentialDriveVoltageConstraint autoVoltageConstraint(
+      frc::SimpleMotorFeedforward<units::meters>(
+          DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
+      DriveConstants::kDriveKinematics, 10_V);
+
+  // Set up config for trajectory
+  frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
+                               AutoConstants::kMaxAcceleration);
+  // Add kinematics to ensure max speed is actually obeyed
+  config.SetKinematics(DriveConstants::kDriveKinematics);
+  // Apply the voltage constraint
+  config.AddConstraint(autoVoltageConstraint);
+
+  // An example trajectory to follow.  All units in meters.
+  auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      // Start at the origin facing the +X direction
+      frc::Pose2d(),
+      // Pass through these two interior waypoints, making an 's' curve path
+      {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
+      // End 3 meters straight ahead of where we started, facing forward
+      frc::Pose2d(3_m, 0_m, 0_deg),
+      // Pass the config
+      config);
+
+  frc2::RamseteCommand ramseteCommand(
+      exampleTrajectory, [this] { return m_drive.GetPose(); },
+      frc::RamseteController(AutoConstants::kRamseteB,
+                             AutoConstants::kRamseteZeta),
+      frc::SimpleMotorFeedforward<units::meters>(
+          DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
+      DriveConstants::kDriveKinematics,
+      [this] { return m_drive.GetWheelSpeeds(); },
+      frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
+      frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
+      [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
+      {&m_drive});
+
+  // Reset odometry to starting pose of trajectory.
+  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+
+  // no auto
+  return new frc2::SequentialCommandGroup(
+      std::move(ramseteCommand),
+      frc2::InstantCommand([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
+}

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

@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+#include <frc/RobotController.h>
+#include <frc/SPI.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
+#include <frc/simulation/SimDeviceSim.h>
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem() {
+  // Set the distance per pulse for the encoders
+  m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+
+  ResetEncoders();
+}
+
+void DriveSubsystem::Periodic() {
+  // Implementation of subsystem periodic method goes here.
+  m_odometry.Update(m_gyro.GetRotation2d(),
+                    units::meter_t(m_leftEncoder.GetDistance()),
+                    units::meter_t(m_rightEncoder.GetDistance()));
+}
+
+void DriveSubsystem::SimulationPeriodic() {
+  // To update our simulation, we set motor voltage inputs, update the
+  // simulation, and write the simulated positions and velocities to our
+  // simulated encoder and gyro. We negate the right side so that positive
+  // voltages make the right side move forward.
+  m_drivetrainSimulator.SetInputs(units::volt_t{m_leftMotors.Get()} *
+                                      frc::RobotController::GetInputVoltage(),
+                                  units::volt_t{-m_rightMotors.Get()} *
+                                      frc::RobotController::GetInputVoltage());
+  m_drivetrainSimulator.Update(20_ms);
+
+  m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetState(
+      frc::sim::DifferentialDrivetrainSim::State::kLeftPosition));
+  m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetState(
+      frc::sim::DifferentialDrivetrainSim::State::kLeftVelocity));
+  m_rightEncoderSim.SetDistance(m_drivetrainSimulator.GetState(
+      frc::sim::DifferentialDrivetrainSim::State::kRightPosition));
+  m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetState(
+      frc::sim::DifferentialDrivetrainSim::State::kRightVelocity));
+  m_gyroAngleSim.SetAngle(
+      -m_drivetrainSimulator.GetHeading().Degrees().to<double>());
+
+  m_fieldSim.SetRobotPose(m_odometry.GetPose());
+}
+
+units::ampere_t DriveSubsystem::GetCurrentDraw() const {
+  return m_drivetrainSimulator.GetCurrentDraw();
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+  m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
+  m_leftMotors.SetVoltage(left);
+  m_rightMotors.SetVoltage(-right);
+  m_drive.Feed();
+}
+
+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);
+}
+
+units::degree_t DriveSubsystem::GetHeading() const {
+  return m_gyro.GetRotation2d().Degrees();
+}
+
+double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); }
+
+frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+
+frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
+  return {units::meters_per_second_t(m_leftEncoder.GetRate()),
+          units::meters_per_second_t(m_rightEncoder.GetRate())};
+}
+
+void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
+  ResetEncoders();
+  m_drivetrainSimulator.SetPose(pose);
+  m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
+}

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

@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/system/plant/DCMotor.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
+#include <wpi/math>
+
+#pragma once
+
+/**
+ * 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 auto kTrackwidth = 0.69_m;
+extern const frc::DifferentialDriveKinematics kDriveKinematics;
+
+constexpr int kEncoderCPR = 1024;
+constexpr auto kWheelDiameter = 6_in;
+constexpr double kEncoderDistancePerPulse =
+    // Assumes the encoders are directly mounted on the wheel shafts
+    (kWheelDiameter.to<double>() * wpi::math::pi) /
+    static_cast<double>(kEncoderCPR);
+
+// 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 Robot Characterization
+// Toolsuite provides a convenient tool for obtaining these values for your
+// robot.
+constexpr auto ks = 0.22_V;
+constexpr auto kv = 1.98 * 1_V * 1_s / 1_m;
+constexpr auto ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
+
+constexpr auto kvAngular = 1.5 * 1_V * 1_s / 1_rad;
+constexpr auto kaAngular = 0.3 * 1_V * 1_s * 1_s / 1_rad;
+
+extern const frc::LinearSystem<2, 2, 2> kDrivetrainPlant;
+
+// Example values only -- use what's on your physical robot!
+constexpr auto kDrivetrainGearbox = frc::DCMotor::CIM(2);
+constexpr auto kDrivetrainGearing = 8.0;
+
+// Example value only - as above, this must be tuned for your drive!
+constexpr double kPDriveVel = 8.5;
+}  // namespace DriveConstants
+
+namespace AutoConstants {
+constexpr auto kMaxSpeed = 3_mps;
+constexpr auto kMaxAcceleration = 3_mps_sq;
+
+// Reasonable baseline values for a RAMSETE follower in units of meters and
+// seconds
+constexpr double kRamseteB = 2;
+constexpr double kRamseteZeta = 0.7;
+}  // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+}  // namespace OIConstants

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

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#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;
+  void SimulationPeriodic() 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/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h

@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/PIDCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.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();
+  const DriveSubsystem& GetRobotDrive() const;
+
+ 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); },
+                                        {}};
+
+  // The chooser for the autonomous routines
+  frc::SendableChooser<frc2::Command*> m_chooser;
+
+  void ConfigureButtonBindings();
+};

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

@@ -0,0 +1,175 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <frc/simulation/AnalogGyroSim.h>
+#include <frc/simulation/DifferentialDrivetrainSim.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/simulation/Field2d.h>
+#include <frc2/command/SubsystemBase.h>
+#include <units/voltage.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+  DriveSubsystem();
+
+  /**
+   * Will be called periodically whenever the CommandScheduler runs.
+   */
+  void Periodic() override;
+
+  /**
+   * Will be called periodically during simulation.
+   */
+  void SimulationPeriodic() override;
+
+  // Subsystem methods go here.
+
+  units::ampere_t GetCurrentDraw() const;
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  void ArcadeDrive(double fwd, double rot);
+
+  /**
+   * Controls each side of the drive directly with a voltage.
+   *
+   * @param left the commanded left output
+   * @param right the commanded right output
+   */
+  void TankDriveVolts(units::volt_t left, units::volt_t right);
+
+  /**
+   * 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);
+
+  /**
+   * Returns the heading of the robot.
+   *
+   * @return the robot's heading in degrees, from -180 to 180
+   */
+  units::degree_t GetHeading() const;
+
+  /**
+   * Returns the turn rate of the robot.
+   *
+   * @return The turn rate of the robot, in degrees per second
+   */
+  double GetTurnRate();
+
+  /**
+   * Returns the currently-estimated pose of the robot.
+   *
+   * @return The pose.
+   */
+  frc::Pose2d GetPose();
+
+  /**
+   * Returns the current wheel speeds of the robot.
+   *
+   * @return The current wheel speeds.
+   */
+  frc::DifferentialDriveWheelSpeeds GetWheelSpeeds();
+
+  /**
+   * Resets the odometry to the specified pose.
+   *
+   * @param pose The pose to which to set the odometry.
+   */
+  void ResetOdometry(frc::Pose2d pose);
+
+ 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{DriveConstants::kLeftMotor1Port};
+  frc::PWMVictorSPX m_left2{DriveConstants::kLeftMotor2Port};
+  frc::PWMVictorSPX m_right1{DriveConstants::kRightMotor1Port};
+  frc::PWMVictorSPX m_right2{DriveConstants::kRightMotor2Port};
+
+  // 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{DriveConstants::kLeftEncoderPorts[0],
+                             DriveConstants::kLeftEncoderPorts[1]};
+
+  // The right-side drive encoder
+  frc::Encoder m_rightEncoder{DriveConstants::kRightEncoderPorts[0],
+                              DriveConstants::kRightEncoderPorts[1]};
+
+  // The gyro sensor
+  frc::AnalogGyro m_gyro{0};
+
+  // Odometry class for tracking robot pose
+  frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
+
+  // These classes help simulate our drivetrain.
+  frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
+      DriveConstants::kDrivetrainPlant, DriveConstants::kTrackwidth,
+      DriveConstants::kDrivetrainGearbox, DriveConstants::kDrivetrainGearing,
+      DriveConstants::kWheelDiameter / 2};
+
+  frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
+  frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
+  frc::sim::AnalogGyroSim m_gyroAngleSim{m_gyro};
+
+  // The Field2d class simulates the field in the sim GUI. Note that we can have
+  // only one instance!
+  frc::Field2d m_fieldSim;
+};

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

@@ -33,10 +33,10 @@ class Robot : public frc::TimedRobot {
   static constexpr units::radians_per_second_t kSpinup = 500_rpm;
 
   // Volts per (radian per second)
-  static constexpr double kFlywheelKv = 0.023;
+  static constexpr auto kFlywheelKv = 0.023_V / 1_rad_per_s;
 
   // Volts per (radian per second squared)
-  static constexpr double kFlywheelKa = 0.001;
+  static constexpr auto kFlywheelKa = 0.001_V / 1_rad_per_s_sq;
 
   // The plant holds a state-space model of our flywheel. This system has the
   // following properties:
@@ -47,7 +47,8 @@ class Robot : public frc::TimedRobot {
   //
   // The Kv and Ka constants are found using the FRC Characterization toolsuite.
   frc::LinearSystem<1, 1, 1> m_flywheelPlant =
-      frc::LinearSystemId::IdentifyVelocitySystem(kFlywheelKv, kFlywheelKa);
+      frc::LinearSystemId::IdentifyVelocitySystem<units::radian>(kFlywheelKv,
+                                                                 kFlywheelKa);
 
   // The observer fuses our encoder data and voltage inputs to reject noise.
   frc::KalmanFilter<1, 1, 1> m_observer{

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

@@ -83,6 +83,9 @@ frc2::Command* RobotContainer::GetAutonomousCommand() {
 
       {&m_drive});
 
+  // Reset odometry to the starting pose of the trajectory.
+  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+
   // no auto
   return new frc2::SequentialCommandGroup(
       std::move(swerveControllerCommand), std::move(swerveControllerCommand),

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

@@ -10,7 +10,7 @@
     ],
     "foldername": "MotorControl",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Motor Control With Encoder",
@@ -25,7 +25,7 @@
     ],
     "foldername": "MotorControlEncoder",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Relay",
@@ -37,7 +37,7 @@
     ],
     "foldername": "Relay",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "PDP CAN Monitoring",
@@ -49,7 +49,7 @@
     ],
     "foldername": "CANPDP",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Solenoids",
@@ -62,7 +62,7 @@
     ],
     "foldername": "Solenoid",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Encoder",
@@ -74,7 +74,7 @@
     ],
     "foldername": "Encoder",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Arcade Drive",
@@ -87,7 +87,7 @@
     ],
     "foldername": "ArcadeDrive",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Mecanum Drive",
@@ -100,7 +100,7 @@
     ],
     "foldername": "MecanumDrive",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Ultrasonic",
@@ -113,7 +113,7 @@
     ],
     "foldername": "Ultrasonic",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "UltrasonicPID",
@@ -126,7 +126,7 @@
     ],
     "foldername": "UltrasonicPID",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Gyro",
@@ -140,7 +140,7 @@
     ],
     "foldername": "Gyro",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Gyro Mecanum",
@@ -154,7 +154,7 @@
     ],
     "foldername": "GyroMecanum",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "HID Rumble",
@@ -164,7 +164,7 @@
     ],
     "foldername": "HidRumble",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "PotentiometerPID",
@@ -178,7 +178,7 @@
     ],
     "foldername": "PotentiometerPID",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Elevator with trapezoid profiled PID",
@@ -191,7 +191,7 @@
     ],
     "foldername": "ElevatorTrapezoidProfile",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Elevator with profiled PID controller",
@@ -204,7 +204,7 @@
     ],
     "foldername": "ElevatorProfiledPID",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Getting Started",
@@ -215,7 +215,7 @@
     ],
     "foldername": "GettingStarted",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Simple Vision",
@@ -226,7 +226,7 @@
     ],
     "foldername": "QuickVision",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Intermediate Vision",
@@ -237,7 +237,7 @@
     ],
     "foldername": "IntermediateVision",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Axis Camera Sample",
@@ -248,7 +248,7 @@
     ],
     "foldername": "AxisCameraSample",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "GearsBot",
@@ -280,7 +280,7 @@
     ],
     "foldername": "HAL",
     "gradlebase": "c",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "ShuffleBoard",
@@ -290,7 +290,7 @@
     ],
     "foldername": "ShuffleBoard",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "'Traditional' Hatchbot",
@@ -414,7 +414,7 @@
     ],
     "foldername": "ArcadeDriveXboxController",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Tank Drive Xbox Controller",
@@ -427,7 +427,7 @@
     ],
     "foldername": "TankDriveXboxController",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Duty Cycle Encoder",
@@ -545,7 +545,7 @@
   },
   {
     "name": "StateSpaceFlywheel",
-    "description": "An example command-based robot demonstrating the use of a SwerveControllerCommand to follow a pregenerated trajectory.",
+    "description": "An example state-space controller for a flywheel.",
     "tags": [
       "StateSpaceFlywheel",
       "Flywheel",
@@ -582,7 +582,7 @@
   },
   {
     "name": "StateSpaceElevator",
-    "description": "An example state-space controller demonstrating the use of FRC Characterization's System Identification for controlling a flywheel.",
+    "description": "An example state-space controller for controlling an elevator.",
     "tags": [
       "Elevator",
       "State Space",
@@ -598,7 +598,7 @@
   },
   {
     "name": "StateSpaceArm",
-    "description": "An example state-space controller demonstrating the use of FRC Characterization's System Identification for controlling a flywheel.",
+    "description": "An example state-space controller for controlling an arm.",
     "tags": [
       "Arm",
       "State space",
@@ -645,7 +645,7 @@
     "commandversion": 2
   },
   {
-    "name": "DifferentialDriveSimulation",
+    "name": "StateSpaceDriveSimulation",
     "description": "Demonstrates the use of physics simulation with a differential drivetrain and the Field2d class.",
     "tags": [
       "Differential Drive",
@@ -657,7 +657,7 @@
       "Drivetrain",
       "Field2d"
     ],
-    "foldername": "DifferentialDriveSimulation",
+    "foldername": "StateSpaceDifferentialDriveSimulation",
     "gradlebase": "cpp",
     "mainclass": "Main",
     "commandversion": 2

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

@@ -159,7 +159,8 @@ public class AnalogPotentiometer implements Potentiometer, Sendable, AutoCloseab
   @Override
   public void initSendable(SendableBuilder builder) {
     if (m_analogInput != null) {
-      m_analogInput.initSendable(builder);
+      builder.setSmartDashboardType("Analog Input");
+      builder.addDoubleProperty("Value", this::get, null);
     }
   }
 

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

@@ -165,6 +165,8 @@ public class DriverStation {
   private int m_waitForDataCount;
   private final ThreadLocal<Integer> m_lastCount = ThreadLocal.withInitial(() -> 0);
 
+  private boolean m_silenceJoystickWarning;
+
   // Robot state status variables
   private boolean m_userInDisabled;
   private boolean m_userInAutonomous;
@@ -594,6 +596,21 @@ public class DriverStation {
     return HAL.getJoystickAxisType((byte) stick, (byte) axis);
   }
 
+  /**
+   * Returns if a joystick is connected to the Driver Station.
+   *
+   * <p>This makes a best effort guess by looking at the reported number of axis,
+   * buttons, and POVs attached.
+   *
+   * @param stick The joystick port number
+   * @return true if a joystick is connected
+   */
+  public boolean isJoystickConnected(int stick) {
+    return getStickAxisCount(stick) > 0
+      || getStickButtonCount(stick) > 0
+      || getStickPOVCount(stick) > 0;
+  }
+
   /**
    * Gets a value indicating whether the Driver Station requires the robot to be enabled.
    *
@@ -1052,6 +1069,27 @@ public class DriverStation {
     m_waitForDataMutex.unlock();
   }
 
+  /**
+   * Allows the user to specify whether they want joystick connection warnings
+   * to be printed to the console. This setting is ignored when the FMS is
+   * connected -- warnings will always be on in that scenario.
+   *
+   * @param silence Whether warning messages should be silenced.
+   */
+  public void silenceJoystickConnectionWarning(boolean silence) {
+    m_silenceJoystickWarning = silence;
+  }
+
+  /**
+   * Returns whether joystick connection warnings are silenced. This will
+   * always return false when connected to the FMS.
+   *
+   * @return Whether joystick connection warnings are silenced.
+   */
+  public boolean isJoystickConnectionWarningSilenced() {
+    return !isFMSAttached() && m_silenceJoystickWarning;
+  }
+
   /**
    * Copy data from the DS task for the user. If no new data exists, it will just be returned,
    * otherwise the data will be copied from the DS polling loop.
@@ -1126,10 +1164,12 @@ public class DriverStation {
    * the DS.
    */
   private void reportJoystickUnpluggedWarning(String message) {
-    double currentTime = Timer.getFPGATimestamp();
-    if (currentTime > m_nextMessageTime) {
-      reportWarning(message, false);
-      m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+    if (isFMSAttached() || !m_silenceJoystickWarning) {
+      double currentTime = Timer.getFPGATimestamp();
+      if (currentTime > m_nextMessageTime) {
+        reportWarning(message, false);
+        m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+      }
     }
   }
 

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

@@ -217,6 +217,15 @@ public abstract class GenericHID {
     return m_ds.getStickButtonCount(m_port);
   }
 
+  /**
+   * Get if the HID is connected.
+   *
+   * @return true if the HID is connected
+   */
+  public boolean isConnected() {
+    return m_ds.isJoystickConnected(m_port);
+  }
+
   /**
    * Get the type of the HID.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java

@@ -98,6 +98,7 @@ public class DifferentialDrivetrainSim {
     m_currentGearing = m_originalGearing;
 
     m_x = new Matrix<>(Nat.N7(), Nat.N1());
+    m_u = VecBuilder.fill(0, 0);
   }
 
   /**
@@ -192,6 +193,8 @@ public class DifferentialDrivetrainSim {
     m_x.set(State.kX.value, 0, pose.getX());
     m_x.set(State.kY.value, 0, pose.getY());
     m_x.set(State.kHeading.value, 0, pose.getRotation().getRadians());
+    m_x.set(State.kLeftPosition.value, 0, 0);
+    m_x.set(State.kRightPosition.value, 0, 0);
   }
 
   @SuppressWarnings({"DuplicatedCode", "LocalVariableName"})

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimDeviceSim.java

@@ -88,13 +88,13 @@ public class SimDeviceSim {
     return SimDeviceDataJNI.enumerateSimDevices(prefix);
   }
 
-  public CallbackStore registerDeviceCreatedCallback(String prefix, SimDeviceCallback callback, boolean initialNotify) {
+  public static 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);
+  public static CallbackStore registerDeviceFreedCallback(String prefix, SimDeviceCallback callback, boolean initialNotify) {
+    int uid = SimDeviceDataJNI.registerSimDeviceFreedCallback(prefix, callback, initialNotify);
     return new CallbackStore(uid, SimDeviceDataJNI::cancelSimDeviceFreedCallback);
   }
 

wpilibj/src/test/java/edu/wpi/first/wpilibj/DriverStationTest.java

@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.stream.Stream;
+
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+class DriverStationTest {
+  @ParameterizedTest
+  @MethodSource("isConnectedProvider")
+  void testIsConnected(int axisCount, int buttonCount, int povCount, boolean expected) {
+    DriverStationSim.setJoystickAxisCount(1, axisCount);
+    DriverStationSim.setJoystickButtonCount(1, buttonCount);
+    DriverStationSim.setJoystickPOVCount(1, povCount);
+
+    DriverStationSim.notifyNewData();
+
+    assertEquals(expected, DriverStation.getInstance().isJoystickConnected(1));
+  }
+
+  static Stream<Arguments> isConnectedProvider() {
+    return Stream.of(
+      arguments(0, 0, 0, false),
+      arguments(1, 0, 0, true),
+      arguments(0, 1, 0, true),
+      arguments(0, 0, 1, true),
+      arguments(1, 1, 1, true),
+      arguments(4, 10, 1, true)
+    );
+  }
+
+  @MethodSource("connectionWarningProvider")
+  void testConnectionWarnings(boolean fms, boolean silence, boolean expected) {
+    DriverStationSim.setFmsAttached(fms);
+    DriverStationSim.notifyNewData();
+
+    DriverStation.getInstance().silenceJoystickConnectionWarning(silence);
+    assertEquals(expected,
+        DriverStation.getInstance().isJoystickConnectionWarningSilenced());
+  }
+
+  static Stream<Arguments> connectionWarningProvider() {
+    return Stream.of(
+      arguments(false, true, true),
+      arguments(false, false, false),
+      arguments(true, true, false),
+      arguments(true, false, false)
+    );
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java

@@ -12,20 +12,77 @@ import org.junit.jupiter.api.Test;
 import edu.wpi.first.hal.SimBoolean;
 import edu.wpi.first.hal.SimDevice;
 
+import java.util.concurrent.atomic.AtomicInteger;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
 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);
+    try (SimDevice dev = SimDevice.create("test")) {
+      SimBoolean devBool = dev.createBoolean("bool", false, false);
 
-    SimDeviceSim sim = new SimDeviceSim("test");
-    SimBoolean simBool = sim.getBoolean("bool");
+      SimDeviceSim sim = new SimDeviceSim("test");
+      SimBoolean simBool = sim.getBoolean("bool");
 
-    assertFalse(simBool.get());
-    simBool.set(true);
-    assertTrue(devBool.get());
+      assertFalse(simBool.get());
+      simBool.set(true);
+      assertTrue(devBool.get());
+    }
+  }
+
+  @Test
+  void testDeviceCreatedCallback() {
+    AtomicInteger callback1Counter = new AtomicInteger(0);
+    AtomicInteger callback2Counter = new AtomicInteger(0);
+
+    try (SimDevice otherdev = SimDevice.create("testnotDC");
+        SimDevice dev1 = SimDevice.create("testDC1")) {
+      SimDeviceSim sim = new SimDeviceSim("testDC1");
+      try (
+          CallbackStore callback1 = sim.registerDeviceCreatedCallback("testDC", (name, handle) -> {
+            callback1Counter.addAndGet(1);
+          }, false);
+          CallbackStore callback2 = sim.registerDeviceCreatedCallback("testDC", (name, handle) -> {
+            callback2Counter.addAndGet(1);
+          }, true)) {
+        assertEquals(0, callback1Counter.get(), "Callback 1 called early");
+        assertEquals(1, callback2Counter.get(), "Callback 2 called early or not initalized with existing devices");
+
+        SimDevice dev2 = SimDevice.create("testDC2");
+        dev2.close();
+
+        assertEquals(1, callback1Counter.get(), "Callback 1 called either more than once or not at all");
+        assertEquals(2, callback2Counter.get(), "Callback 2 called either more or less than twice");
+      }
+
+      SimDevice dev3 = SimDevice.create("testDC3");
+      dev3.close();
+
+      assertEquals(1, callback1Counter.get(), "Callback 1 called after closure");
+      assertEquals(2, callback2Counter.get(), "Callback 2 called after closure");
+    }
+  }
+
+  @Test
+  void testDeviceFreedCallback() {
+    AtomicInteger counter = new AtomicInteger(0);
+
+    SimDevice dev1 = SimDevice.create("testDF1");
+    SimDeviceSim sim = new SimDeviceSim("testDF1");
+    try (CallbackStore callback = sim.registerDeviceFreedCallback("testDF", (name, handle) -> {
+      counter.addAndGet(1);
+    }, false)) {
+      assertEquals(0, counter.get(), "Callback called early");
+      dev1.close();
+      assertEquals(1, counter.get(), "Callback called either more than once or not at all");
+    }
+
+    SimDevice dev2 = SimDevice.create("testDF2");
+    dev2.close();
+
+    assertEquals(1, counter.get(), "Callback called after closure");
   }
 }

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

@@ -8,7 +8,7 @@
     "foldername": "gettingstarted",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Tank Drive",
@@ -22,7 +22,7 @@
     "foldername": "tankdrive",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Arcade Drive",
@@ -33,7 +33,7 @@
     "foldername": "arcadedrive",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Mecanum Drive",
@@ -47,7 +47,7 @@
     "foldername": "mecanumdrive",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "PDP CAN Monitoring",
@@ -60,7 +60,7 @@
     "foldername": "canpdp",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Solenoids",
@@ -74,7 +74,7 @@
     "foldername": "solenoid",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Encoder",
@@ -87,7 +87,7 @@
     "foldername": "encoder",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Relay",
@@ -100,7 +100,7 @@
     "foldername": "relay",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Ultrasonic",
@@ -113,7 +113,7 @@
     "foldername": "ultrasonic",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Ultrasonic PID",
@@ -126,7 +126,7 @@
     "foldername": "ultrasonicpid",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Potentiometer PID",
@@ -140,7 +140,7 @@
     "foldername": "potentiometerpid",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Elevator with trapezoid profiled PID",
@@ -154,7 +154,7 @@
     "foldername": "elevatortrapezoidprofile",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Elevator with profiled PID controller",
@@ -168,7 +168,7 @@
     "foldername": "elevatorprofiledpid",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Gyro",
@@ -182,7 +182,7 @@
     "foldername": "gyro",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Gyro Mecanum",
@@ -196,7 +196,7 @@
     "foldername": "gyromecanum",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "HID Rumble",
@@ -207,7 +207,7 @@
     "foldername": "hidrumble",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Motor Controller",
@@ -220,7 +220,7 @@
     "foldername": "motorcontrol",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Motor Control With Encoder",
@@ -236,7 +236,7 @@
     "foldername": "motorcontrolencoder",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "GearsBot",
@@ -270,7 +270,7 @@
     "foldername": "quickvision",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Intermediate Vision",
@@ -282,7 +282,7 @@
     "foldername": "intermediatevision",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Axis Camera Sample",
@@ -293,7 +293,7 @@
     "foldername": "axiscamera",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Shuffleboard Sample",
@@ -305,7 +305,7 @@
     "foldername": "shuffleboard",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "'Traditional' Hatchbot",
@@ -437,7 +437,7 @@
     "foldername": "arcadedrivexboxcontroller",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Tank Drive Xbox Controller",
@@ -448,7 +448,7 @@
     "foldername": "tankdrivexboxcontroller",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Duty Cycle Encoder",
@@ -618,7 +618,7 @@
   },
   {
     "name": "StateSpaceArm",
-    "description": "An example state-space controller demonstrating the use of FRC Characterization's System Identification for controlling a flywheel.",
+    "description": "An example state-space controller for controlling an arm.",
     "tags": [
       "Arm",
       "State space",
@@ -633,8 +633,8 @@
     "commandversion": 2
   },
   {
-    "name": "StateSpaceSimulation",
-    "description": "An example of drivetrain simulation in combination with a RAMSETE path following controller.",
+    "name": "StateSpaceDriveSimulation",
+    "description": "An example of drivetrain simulation in combination with a RAMSETE path following controller and the Field2d class.",
     "tags": [
       "Drivetrain",
       "State space",

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -133,6 +133,9 @@ public class RobotContainer {
         m_robotDrive
     );
 
+    // Reset odometry to the starting pose of the trajectory.
+    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
+
     // Run path following command, then stop at the end.
     return mecanumControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -135,6 +135,9 @@ public class RobotContainer {
         m_robotDrive
     );
 
+    // Reset odometry to the starting pose of the trajectory.
+    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
+
     // Run path following command, then stop at the end.
     return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java

@@ -130,6 +130,9 @@ public class RobotContainer {
         m_robotDrive
     );
 
+    // Reset odometry to starting pose of trajectory.
+    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
+
     // Run path following command, then stop at the end.
     return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java

@@ -163,6 +163,7 @@ public class DriveSubsystem extends SubsystemBase {
    */
   public void resetOdometry(Pose2d pose) {
     resetEncoders();
+    m_drivetrainSimulator.setPose(pose);
     m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
   }
 

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java

@@ -117,6 +117,9 @@ public class RobotContainer {
 
     );
 
+    // Reset odometry to the starting pose of the trajectory.
+    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
+
     // Run path following command, then stop at the end.
     return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
   }

wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java

@@ -272,29 +272,30 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num,
   }
 
   /**
-   * Zeroes reference r, controller output u and plant output y.
-   * The previous reference for PlantInversionFeedforward is set to the
-   * initial reference.
-   * @param initialReference The initial reference.
+   * Zeroes reference r and controller output u. The previous reference
+   * of the PlantInversionFeedforward and the initial state estimate of
+   * the KalmanFilter are set to the initial state provided.
+   *
+   * @param initialState The initial state.
    */
-  public void reset(Matrix<States, N1> initialReference) {
-    m_controller.reset();
-    m_feedforward.reset(initialReference);
-    m_observer.reset();
+  public void reset(Matrix<States, N1> initialState) {
     m_nextR.fill(0.0);
+    m_controller.reset();
+    m_feedforward.reset(initialState);
+    m_observer.setXhat(initialState);
   }
 
   /**
-   * Returns difference between reoid predict(double dtSference r and x-hat.
+   * Returns difference between reference r and current state x-hat.
    *
-   * @return the
+   * @return The state error matrix.
    */
   public Matrix<States, N1> getError() {
     return getController().getR().minus(m_observer.getXhat());
   }
 
   /**
-   * Returns difference between reference r and x-hat.
+   * Returns difference between reference r and current state x-hat.
    *
    * @param index The index of the error matrix to return.
    * @return The error at that index.

wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java

@@ -134,6 +134,9 @@ public final class LinearSystemId {
    * Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2).
    * These constants cam be found using frc-characterization.
    *
+   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use
+   * the {@link edu.wpi.first.wpilibj.util.Units} class for converting between unit types.
+   *
    * @param kV         The velocity gain, in volts per (units per second)
    * @param kA         The acceleration gain, in volts per (units per second squared)
    * @return A LinearSystem representing the given characterized constants.
@@ -153,6 +156,9 @@ public final class LinearSystemId {
    * Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2).
    * These constants cam be found using frc-characterization.
    *
+   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use
+   * the {@link edu.wpi.first.wpilibj.util.Units} class for converting between unit types.
+   *
    * @param kV         The velocity gain, in volts per (units per second)
    * @param kA         The acceleration gain, in volts per (units per second squared)
    * @return A LinearSystem representing the given characterized constants.

wpimath/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp

@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/trajectory/constraint/EllipticalRegionConstraint.h"
-
-#include <limits>
-
-#include "units/math.h"
-
-using namespace frc;
-
-EllipticalRegionConstraint::EllipticalRegionConstraint(
-    const Translation2d& center, units::meter_t xWidth, units::meter_t yWidth,
-    const Rotation2d& rotation, const TrajectoryConstraint& constraint)
-    : m_center(center),
-      m_radii(xWidth / 2.0, yWidth / 2.0),
-      m_constraint(constraint) {
-  m_radii = m_radii.RotateBy(rotation);
-}
-
-units::meters_per_second_t EllipticalRegionConstraint::MaxVelocity(
-    const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t velocity) const {
-  if (IsPoseInRegion(pose)) {
-    return m_constraint.MaxVelocity(pose, curvature, velocity);
-  } else {
-    return units::meters_per_second_t(std::numeric_limits<double>::infinity());
-  }
-}
-
-TrajectoryConstraint::MinMax EllipticalRegionConstraint::MinMaxAcceleration(
-    const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t speed) const {
-  if (IsPoseInRegion(pose)) {
-    return m_constraint.MinMaxAcceleration(pose, curvature, speed);
-  } else {
-    return {};
-  }
-}
-
-bool EllipticalRegionConstraint::IsPoseInRegion(const Pose2d& pose) const {
-  // The region (disk) bounded by the ellipse is given by the equation:
-  // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
-  // If the inequality is satisfied, then it is inside the ellipse; otherwise
-  // it is outside the ellipse.
-  // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
-  return units::math::pow<2>(pose.X() - m_center.X()) *
-                 units::math::pow<2>(m_radii.Y()) +
-             units::math::pow<2>(pose.Y() - m_center.Y()) *
-                 units::math::pow<2>(m_radii.X()) <=
-         units::math::pow<2>(m_radii.X()) * units::math::pow<2>(m_radii.Y());
-}

wpimath/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp

@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/trajectory/constraint/RectangularRegionConstraint.h"
-
-#include <limits>
-
-using namespace frc;
-
-RectangularRegionConstraint::RectangularRegionConstraint(
-    const Translation2d& bottomLeftPoint, const Translation2d& topRightPoint,
-    const TrajectoryConstraint& constraint)
-    : m_bottomLeftPoint(bottomLeftPoint),
-      m_topRightPoint(topRightPoint),
-      m_constraint(constraint) {}
-
-units::meters_per_second_t RectangularRegionConstraint::MaxVelocity(
-    const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t velocity) const {
-  if (IsPoseInRegion(pose)) {
-    return m_constraint.MaxVelocity(pose, curvature, velocity);
-  } else {
-    return units::meters_per_second_t(std::numeric_limits<double>::infinity());
-  }
-}
-
-TrajectoryConstraint::MinMax RectangularRegionConstraint::MinMaxAcceleration(
-    const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t speed) const {
-  if (IsPoseInRegion(pose)) {
-    return m_constraint.MinMaxAcceleration(pose, curvature, speed);
-  } else {
-    return {};
-  }
-}
-
-bool RectangularRegionConstraint::IsPoseInRegion(const Pose2d& pose) const {
-  return pose.X() >= m_bottomLeftPoint.X() && pose.X() <= m_topRightPoint.X() &&
-         pose.Y() >= m_bottomLeftPoint.Y() && pose.Y() <= m_topRightPoint.Y();
-}

wpimath/src/main/native/include/frc/system/LinearSystemLoop.h

@@ -227,20 +227,21 @@ class LinearSystemLoop {
   }
 
   /**
-   * Zeroes reference r, controller output u and plant output y.
-   * The previous reference for PlantInversionFeedforward is set to the
-   * initial reference.
-   * @param initialReference The initial reference.
+   * Zeroes reference r and controller output u. The previous reference
+   * of the PlantInversionFeedforward and the initial state estimate of
+   * the KalmanFilter are set to the initial state provided.
+   *
+   * @param initialState The initial state.
    */
   void Reset(Eigen::Matrix<double, States, 1> initialState) {
+    m_nextR.setZero();
     m_controller.Reset();
     m_feedforward.Reset(initialState);
-    m_observer.Reset();
-    m_nextR.setZero();
+    m_observer.SetXhat(initialState);
   }
 
   /**
-   * Returns difference between reference r and x-hat.
+   * Returns difference between reference r and current state x-hat.
    */
   const Eigen::Matrix<double, States, 1> Error() const {
     return m_controller.R() - m_observer.Xhat();

wpimath/src/main/native/include/frc/system/plant/DCMotor.h

@@ -46,7 +46,7 @@ class DCMotor {
    * Constructs a DC motor.
    *
    * @param nominalVoltage Voltage at which the motor constants were measured.
-   * @param stallTorque    Current draw when stalled.
+   * @param stallTorque    Torque when stalled.
    * @param stallCurrent   Current draw when stalled.
    * @param freeCurrent    Current draw under no load.
    * @param freeSpeed      Angular velocity under no load.

wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h

@@ -10,12 +10,25 @@
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/LinearSystem.h"
 #include "frc/system/plant/DCMotor.h"
+#include "units/acceleration.h"
+#include "units/angular_acceleration.h"
+#include "units/angular_velocity.h"
 #include "units/moment_of_inertia.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
 
 namespace frc {
-
 class LinearSystemId {
  public:
+  template <typename Distance>
+  using Velocity_t = units::unit_t<
+      units::compound_unit<Distance, units::inverse<units::seconds>>>;
+
+  template <typename Distance>
+  using Acceleration_t = units::unit_t<units::compound_unit<
+      units::compound_unit<Distance, units::inverse<units::seconds>>,
+      units::inverse<units::seconds>>>;
+
   /**
    * Constructs the state-space model for an elevator.
    *
@@ -72,6 +85,11 @@ class LinearSystemId {
    * Constructs the state-space model for a 1 DOF velocity-only system from
    * system identification data.
    *
+   * You MUST use an SI unit (i.e. meters or radians) for the Distance template
+   * argument. You may still use non-SI units (such as feet or inches) for the
+   * actual method arguments; they will automatically be converted to SI
+   * internally.
+   *
    * States: [[velocity]]
    * Inputs: [[voltage]]
    * Outputs: [[velocity]]
@@ -83,9 +101,15 @@ class LinearSystemId {
    * @param kV The velocity gain, in volt seconds per distance.
    * @param kA The acceleration gain, in volt seconds^2 per distance.
    */
-  static LinearSystem<1, 1, 1> IdentifyVelocitySystem(double kV, double kA) {
-    auto A = frc::MakeMatrix<1, 1>(-kV / kA);
-    auto B = frc::MakeMatrix<1, 1>(1.0 / kA);
+  template <typename Distance, typename = std::enable_if_t<
+                                   std::is_same_v<units::meter, Distance> ||
+                                   std::is_same_v<units::radian, Distance>>>
+  static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
+      decltype(1_V / Velocity_t<Distance>(1)) kV,
+      decltype(1_V / Acceleration_t<Distance>(1)) kA) {
+    auto A = frc::MakeMatrix<1, 1>(-kV.template to<double>() /
+                                   kA.template to<double>());
+    auto B = frc::MakeMatrix<1, 1>(1.0 / kA.template to<double>());
     auto C = frc::MakeMatrix<1, 1>(1.0);
     auto D = frc::MakeMatrix<1, 1>(0.0);
 
@@ -96,6 +120,11 @@ class LinearSystemId {
    * Constructs the state-space model for a 1 DOF position system from system
    * identification data.
    *
+   * You MUST use an SI unit (i.e. meters or radians) for the Distance template
+   * argument. You may still use non-SI units (such as feet or inches) for the
+   * actual method arguments; they will automatically be converted to SI
+   * internally.
+   *
    * States: [[position], [velocity]]
    * Inputs: [[voltage]]
    * Outputs: [[position]]
@@ -107,9 +136,15 @@ class LinearSystemId {
    * @param kV The velocity gain, in volt seconds per distance.
    * @param kA The acceleration gain, in volt seconds^2 per distance.
    */
-  static LinearSystem<2, 1, 1> IdentifyPositionSystem(double kV, double kA) {
-    auto A = frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kV / kA);
-    auto B = frc::MakeMatrix<2, 1>(0.0, 1.0 / kA);
+  template <typename Distance, typename = std::enable_if_t<
+                                   std::is_same_v<units::meter, Distance> ||
+                                   std::is_same_v<units::radian, Distance>>>
+  static LinearSystem<2, 1, 1> IdentifyPositionSystem(
+      decltype(1_V / Velocity_t<Distance>(1)) kV,
+      decltype(1_V / Acceleration_t<Distance>(1)) kA) {
+    auto A = frc::MakeMatrix<2, 2>(
+        0.0, 1.0, 0.0, -kV.template to<double>() / kA.template to<double>());
+    auto B = frc::MakeMatrix<2, 1>(0.0, 1.0 / kA.template to<double>());
     auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
     auto D = frc::MakeMatrix<1, 1>(0.0);
 
@@ -131,15 +166,17 @@ class LinearSystemId {
    * @param kAangular The angular acceleration gain, in volt seconds^2 per
    * angle.
    */
-  static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(double kVlinear,
-                                                        double kAlinear,
-                                                        double kVangular,
-                                                        double kAangular) {
-    double c = 0.5 / (kAlinear * kAangular);
-    double A1 = c * (-kAlinear * kVangular - kVlinear * kAangular);
-    double A2 = c * (kAlinear * kVangular - kVlinear * kAangular);
-    double B1 = c * (kAlinear + kAangular);
-    double B2 = c * (kAangular - kAlinear);
+  static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
+      decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear,
+      decltype(1_V / 1_rad_per_s) kVangular,
+      decltype(1_V / 1_rad_per_s_sq) kAangular) {
+    double c = 0.5 / (kAlinear.to<double>() * kAangular.to<double>());
+    double A1 = c * (-kAlinear.to<double>() * kVangular.to<double>() -
+                     kVlinear.to<double>() * kAangular.to<double>());
+    double A2 = c * (kAlinear.to<double>() * kVangular.to<double>() -
+                     kVlinear.to<double>() * kAangular.to<double>());
+    double B1 = c * (kAlinear.to<double>() + kAangular.to<double>());
+    double B2 = c * (kAangular.to<double>() - kAlinear.to<double>());
 
     auto A = frc::MakeMatrix<2, 2>(A1, A2, A2, A1);
     auto B = frc::MakeMatrix<2, 2>(B1, B2, B2, B1);

wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h

@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <limits>
+
 #include "frc/geometry/Rotation2d.h"
 #include "frc/geometry/Translation2d.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
@@ -16,6 +18,8 @@ namespace frc {
 /**
  * Enforces a particular constraint only within an elliptical region.
  */
+template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
+                                   TrajectoryConstraint, Constraint>>>
 class EllipticalRegionConstraint : public TrajectoryConstraint {
  public:
   /**
@@ -30,14 +34,32 @@ class EllipticalRegionConstraint : public TrajectoryConstraint {
    */
   EllipticalRegionConstraint(const Translation2d& center, units::meter_t xWidth,
                              units::meter_t yWidth, const Rotation2d& rotation,
-                             const TrajectoryConstraint& constraint);
+                             const Constraint& constraint)
+      : m_center(center),
+        m_radii(xWidth / 2.0, yWidth / 2.0),
+        m_constraint(constraint) {
+    m_radii = m_radii.RotateBy(rotation);
+  }
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
-      units::meters_per_second_t velocity) const override;
+      units::meters_per_second_t velocity) const override {
+    if (IsPoseInRegion(pose)) {
+      return m_constraint.MaxVelocity(pose, curvature, velocity);
+    } else {
+      return units::meters_per_second_t(
+          std::numeric_limits<double>::infinity());
+    }
+  }
 
   MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
-                            units::meters_per_second_t speed) const override;
+                            units::meters_per_second_t speed) const override {
+    if (IsPoseInRegion(pose)) {
+      return m_constraint.MinMaxAcceleration(pose, curvature, speed);
+    } else {
+      return {};
+    }
+  }
 
   /**
    * Returns whether the specified robot pose is within the region that the
@@ -46,11 +68,22 @@ class EllipticalRegionConstraint : public TrajectoryConstraint {
    * @param pose The robot pose.
    * @return Whether the robot pose is within the constraint region.
    */
-  bool IsPoseInRegion(const Pose2d& pose) const;
+  bool IsPoseInRegion(const Pose2d& pose) const {
+    // The region (disk) bounded by the ellipse is given by the equation:
+    // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
+    // If the inequality is satisfied, then it is inside the ellipse; otherwise
+    // it is outside the ellipse.
+    // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
+    return units::math::pow<2>(pose.X() - m_center.X()) *
+                   units::math::pow<2>(m_radii.Y()) +
+               units::math::pow<2>(pose.Y() - m_center.Y()) *
+                   units::math::pow<2>(m_radii.X()) <=
+           units::math::pow<2>(m_radii.X()) * units::math::pow<2>(m_radii.Y());
+  }
 
  private:
   Translation2d m_center;
   Translation2d m_radii;
-  const TrajectoryConstraint& m_constraint;
+  Constraint m_constraint;
 };
 }  // namespace frc

wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h

@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <limits>
+
 #include "frc/geometry/Rotation2d.h"
 #include "frc/geometry/Translation2d.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
@@ -15,6 +17,8 @@ namespace frc {
 /**
  * Enforces a particular constraint only within a rectangular region.
  */
+template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
+                                   TrajectoryConstraint, Constraint>>>
 class RectangularRegionConstraint : public TrajectoryConstraint {
  public:
   /**
@@ -29,14 +33,30 @@ class RectangularRegionConstraint : public TrajectoryConstraint {
    */
   RectangularRegionConstraint(const Translation2d& bottomLeftPoint,
                               const Translation2d& topRightPoint,
-                              const TrajectoryConstraint& constraint);
+                              const Constraint& constraint)
+      : m_bottomLeftPoint(bottomLeftPoint),
+        m_topRightPoint(topRightPoint),
+        m_constraint(constraint) {}
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
-      units::meters_per_second_t velocity) const override;
+      units::meters_per_second_t velocity) const override {
+    if (IsPoseInRegion(pose)) {
+      return m_constraint.MaxVelocity(pose, curvature, velocity);
+    } else {
+      return units::meters_per_second_t(
+          std::numeric_limits<double>::infinity());
+    }
+  }
 
   MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
-                            units::meters_per_second_t speed) const override;
+                            units::meters_per_second_t speed) const override {
+    if (IsPoseInRegion(pose)) {
+      return m_constraint.MinMaxAcceleration(pose, curvature, speed);
+    } else {
+      return {};
+    }
+  }
 
   /**
    * Returns whether the specified robot pose is within the region that the
@@ -45,11 +65,15 @@ class RectangularRegionConstraint : public TrajectoryConstraint {
    * @param pose The robot pose.
    * @return Whether the robot pose is within the constraint region.
    */
-  bool IsPoseInRegion(const Pose2d& pose) const;
+  bool IsPoseInRegion(const Pose2d& pose) const {
+    return pose.X() >= m_bottomLeftPoint.X() &&
+           pose.X() <= m_topRightPoint.X() &&
+           pose.Y() >= m_bottomLeftPoint.Y() && pose.Y() <= m_topRightPoint.Y();
+  }
 
  private:
   Translation2d m_bottomLeftPoint;
   Translation2d m_topRightPoint;
-  const TrajectoryConstraint& m_constraint;
+  Constraint m_constraint;
 };
 }  // namespace frc

wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp

@@ -53,7 +53,8 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) {
   // x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
   double kv = 1.0;
   double ka = 0.5;
-  auto model = frc::LinearSystemId::IdentifyPositionSystem(kv, ka);
+  auto model = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
+      kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
 
   ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kv / ka),
                                  0.001));
@@ -66,7 +67,8 @@ TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
   // x-dot =  -kv/ka * v + 1/ka \cdot V
   double kv = 1.0;
   double ka = 0.5;
-  auto model = frc::LinearSystemId::IdentifyVelocitySystem(kv, ka);
+  auto model = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
+      kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
 
   ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-kv / ka), 0.001));
   ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1.0 / ka), 0.001));

wpimath/wpimath-config.cmake.in

@@ -2,4 +2,5 @@ include(CMakeFindDependencyMacro)
 @FILENAME_DEP_REPLACE@
 @WPIUTIL_DEP_REPLACE@
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/wpimath.cmake)

wpiutil/src/netconsoleTee/native/cpp/main.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -57,14 +57,14 @@ static bool NewlineBuffer(std::string& rem, uv::Buffer& buf, size_t len,
   return true;
 }
 
-static void CopyUdp(uv::Stream& in, std::shared_ptr<uv::Udp> out,
+static void CopyUdp(uv::Stream& in, std::shared_ptr<uv::Udp> out, int port,
                     bool broadcast) {
   sockaddr_in addr;
   if (broadcast) {
     out->SetBroadcast(true);
-    uv::NameToAddr("0.0.0.0", 6666, &addr);
+    uv::NameToAddr("0.0.0.0", port, &addr);
   } else {
-    uv::NameToAddr("127.0.0.1", 6666, &addr);
+    uv::NameToAddr("127.0.0.1", port, &addr);
   }
 
   in.data.connect(
@@ -119,6 +119,7 @@ int main(int argc, char* argv[]) {
   bool useUdp = false;
   bool broadcastUdp = false;
   bool err = false;
+  int port = -1;
 
   while (arg < argc && argv[arg][0] == '-') {
     if (wpi::StringRef(argv[arg]) == "-u") {
@@ -126,6 +127,13 @@ int main(int argc, char* argv[]) {
     } else if (wpi::StringRef(argv[arg]) == "-b") {
       useUdp = true;
       broadcastUdp = true;
+    } else if (wpi::StringRef(argv[arg]) == "-p") {
+      ++arg;
+      if (arg >= argc || argv[arg][0] == '-' ||
+          wpi::StringRef(argv[arg]).getAsInteger(10, port)) {
+        wpi::errs() << "-p must be followed by port number\n";
+        err = true;
+      }
     } else {
       wpi::errs() << "unrecognized command line option " << argv[arg] << '\n';
       err = true;
@@ -135,9 +143,10 @@ int main(int argc, char* argv[]) {
 
   if (err) {
     wpi::errs()
-        << argv[0] << " [-ub]\n"
-        << "  -u  send udp to localhost port 6666 instead of using tcp\n"
-        << "  -b  broadcast udp to port 6666 instead of using tcp\n";
+        << argv[0] << " [-ub] [-p PORT]\n"
+        << "  -u       send udp to localhost port 6666 instead of using tcp\n"
+        << "  -b       broadcast udp to port 6666 instead of using tcp\n"
+        << "  -p PORT  use port PORT instead of 6666 (udp) or 1740 (tcp)\n";
     return EXIT_FAILURE;
   }
 
@@ -161,12 +170,12 @@ int main(int argc, char* argv[]) {
   if (useUdp) {
     auto udp = uv::Udp::Create(loop);
     // tee
-    CopyUdp(*stdinTty, udp, broadcastUdp);
+    CopyUdp(*stdinTty, udp, port < 0 ? 6666 : port, broadcastUdp);
   } else {
     auto tcp = uv::Tcp::Create(loop);
 
     // bind to listen address and port
-    tcp->Bind("", 1740);
+    tcp->Bind("", port < 0 ? 1740 : port);
 
     // when we get a connection, accept it
     tcp->connection.connect([srv = tcp.get(), stdinTty] {

wpiutil/wpiutil-config.cmake.in

@@ -5,4 +5,5 @@ find_dependency(Threads)
 @LIBUV_VCPKG_REPLACE@
 @EIGEN_VCPKG_REPLACE@
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/wpiutil.cmake)