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

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

.github/workflows/ci.yml

@@ -159,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

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)

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