wpilibc: Remove direct CameraServer dependency (#1989)

This makes linking easier, particularly for third party vendors and other language wrappers.

azure-pipelines.yml

@@ -298,9 +298,6 @@ stages:
           mkdir build
           export JAVA_HOME=`/usr/libexec/java_home -v 11`
         displayName: 'Setup JDK'
-      - script: |
-          rm /Users/vsts/.gradle/init.d/log-gradle-version-plugin.gradle
-        displayName: 'Delete Version init script'
 
       - task: Gradle@2
         condition: and(succeeded(), not(startsWith(variables['Build.SourceBranch'], 'refs/tags/v')))

buildSrc/build.gradle

@@ -5,5 +5,5 @@ repositories {
     }
 }
 dependencies {
-    compile "edu.wpi.first:native-utils:2020.1.2"
+    compile "edu.wpi.first:native-utils:2020.1.4"
 }

cameraserver/src/main/native/cpp/cameraserver/CameraServerShared.cpp

@@ -24,15 +24,10 @@ class DefaultCameraServerShared : public frc::CameraServerShared {
 };
 }  // namespace
 
-namespace frc {
-
-static std::unique_ptr<CameraServerShared> cameraServerShared = nullptr;
+static std::unique_ptr<frc::CameraServerShared> cameraServerShared = nullptr;
 static wpi::mutex setLock;
 
-void SetCameraServerShared(std::unique_ptr<CameraServerShared> shared) {
-  std::unique_lock lock(setLock);
-  cameraServerShared = std::move(shared);
-}
+namespace frc {
 CameraServerShared* GetCameraServerShared() {
   std::unique_lock lock(setLock);
   if (!cameraServerShared) {
@@ -41,3 +36,10 @@ CameraServerShared* GetCameraServerShared() {
   return cameraServerShared.get();
 }
 }  // namespace frc
+
+extern "C" {
+void CameraServer_SetCameraServerShared(frc::CameraServerShared* shared) {
+  std::unique_lock lock(setLock);
+  cameraServerShared.reset(shared);
+}
+}  // extern "C"

cameraserver/src/main/native/include/cameraserver/CameraServerShared.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -26,6 +26,10 @@ class CameraServerShared {
   virtual std::pair<std::thread::id, bool> GetRobotMainThreadId() const = 0;
 };
 
-void SetCameraServerShared(std::unique_ptr<CameraServerShared> shared);
 CameraServerShared* GetCameraServerShared();
 }  // namespace frc
+
+extern "C" {
+// Takes ownership
+void CameraServer_SetCameraServerShared(frc::CameraServerShared* shared);
+}  // extern "C"

cmake/modules/GenResources.cmake

@@ -1,3 +1,4 @@
+set(SCRIPTS_DIR "${CMAKE_CURRENT_LIST_DIR}/../scripts")
 MACRO(GENERATE_RESOURCES inputDir outputDir prefix namespace outputFiles)
   FILE(GLOB inputFiles ${inputDir}/*)
   SET(${outputFiles})
@@ -16,9 +17,9 @@ MACRO(GENERATE_RESOURCES inputDir outputDir prefix namespace outputFiles)
         "-Doutput=${output}"
         "-Dprefix=${prefix}"
         "-Dnamespace=${namespace}"
-        -P "${CMAKE_SOURCE_DIR}/cmake/scripts/GenResource.cmake"
+        -P "${SCRIPTS_DIR}/GenResource.cmake"
       MAIN_DEPENDENCY ${input}
-      DEPENDS ${CMAKE_SOURCE_DIR}/cmake/scripts/GenResource.cmake
+      DEPENDS ${SCRIPTS_DIR}/GenResource.cmake
       VERBATIM
     )
   ENDFOREACH()

cscore/src/main/java/edu/wpi/cscore/UsbCameraInfo.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,13 +18,18 @@ public class UsbCameraInfo {
    * @param path Path to device if available (e.g. '/dev/video0' on Linux)
    * @param name Vendor/model name of the camera as provided by the USB driver
    * @param otherPaths Other path aliases to device
+   * @param vendorId USB vendor id
+   * @param productId USB product id
    */
   @SuppressWarnings("PMD.ArrayIsStoredDirectly")
-  public UsbCameraInfo(int dev, String path, String name, String[] otherPaths) {
+  public UsbCameraInfo(int dev, String path, String name, String[] otherPaths, int vendorId,
+      int productId) {
     this.dev = dev;
     this.path = path;
     this.name = name;
     this.otherPaths = otherPaths;
+    this.vendorId = vendorId;
+    this.productId = productId;
   }
 
   /**
@@ -50,4 +55,16 @@ public class UsbCameraInfo {
    */
   @SuppressWarnings("MemberName")
   public String[] otherPaths;
+
+  /**
+   * USB vendor id.
+   */
+  @SuppressWarnings("MemberName")
+  public int vendorId;
+
+  /**
+   * USB product id.
+   */
+  @SuppressWarnings("MemberName")
+  public int productId;
 }

cscore/src/main/native/cpp/UsbCameraImplCommon.cpp

@@ -21,6 +21,8 @@ static void ConvertToC(CS_UsbCameraInfo* out, const UsbCameraInfo& in) {
   out->otherPathsCount = in.otherPaths.size();
   for (size_t i = 0; i < in.otherPaths.size(); ++i)
     out->otherPaths[i] = cs::ConvertToC(in.otherPaths[i]);
+  out->vendorId = in.vendorId;
+  out->productId = in.productId;
 }
 
 static void FreeUsbCameraInfo(CS_UsbCameraInfo* info) {

cscore/src/main/native/cpp/jni/CameraServerJNI.cpp

@@ -194,13 +194,14 @@ static inline bool CheckStatus(JNIEnv* env, CS_Status status) {
 static jobject MakeJObject(JNIEnv* env, const cs::UsbCameraInfo& info) {
   static jmethodID constructor = env->GetMethodID(
       usbCameraInfoCls, "<init>",
-      "(ILjava/lang/String;Ljava/lang/String;[Ljava/lang/String;)V");
+      "(ILjava/lang/String;Ljava/lang/String;[Ljava/lang/String;II)V");
   JLocal<jstring> path(env, MakeJString(env, info.path));
   JLocal<jstring> name(env, MakeJString(env, info.name));
   JLocal<jobjectArray> otherPaths(env, MakeJStringArray(env, info.otherPaths));
   return env->NewObject(usbCameraInfoCls, constructor,
                         static_cast<jint>(info.dev), path.obj(), name.obj(),
-                        otherPaths.obj());
+                        otherPaths.obj(), static_cast<jint>(info.vendorId),
+                        static_cast<jint>(info.productId));
 }
 
 static jobject MakeJObject(JNIEnv* env, const cs::VideoMode& videoMode) {

cscore/src/main/native/include/cscore_c.h

@@ -236,6 +236,8 @@ typedef struct CS_UsbCameraInfo {
   char* name;
   int otherPathsCount;
   char** otherPaths;
+  int vendorId;
+  int productId;
 } CS_UsbCameraInfo;
 
 /**

cscore/src/main/native/include/cscore_cpp.h

@@ -56,6 +56,10 @@ struct UsbCameraInfo {
   std::string name;
   /** Other path aliases to device (e.g. '/dev/v4l/by-id/...' etc on Linux) */
   std::vector<std::string> otherPaths;
+  /** USB Vendor Id */
+  int vendorId = -1;
+  /** USB Product Id */
+  int productId = -1;
 };
 
 /**

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

@@ -143,10 +143,36 @@ int UsbCameraImpl::PercentageToRaw(const UsbCameraProperty& rawProp,
          (rawProp.maximum - rawProp.minimum) * (percentValue / 100.0);
 }
 
-static bool GetDescriptionSysV4L(wpi::StringRef path, std::string* desc) {
-  wpi::SmallString<64> ifpath{"/sys/class/video4linux/"};
-  ifpath += path.substr(5);
-  ifpath += "/device/interface";
+static bool GetVendorProduct(int dev, int* vendor, int* product) {
+  wpi::SmallString<64> ifpath;
+  {
+    wpi::raw_svector_ostream oss{ifpath};
+    oss << "/sys/class/video4linux/video" << dev << "/device/modalias";
+  }
+
+  int fd = open(ifpath.c_str(), O_RDONLY);
+  if (fd < 0) return false;
+
+  char readBuf[128];
+  ssize_t n = read(fd, readBuf, sizeof(readBuf));
+  close(fd);
+
+  if (n <= 0) return false;
+  wpi::StringRef readStr{readBuf};
+  if (readStr.substr(readStr.find('v')).substr(1, 4).getAsInteger(16, *vendor))
+    return false;
+  if (readStr.substr(readStr.find('p')).substr(1, 4).getAsInteger(16, *product))
+    return false;
+
+  return true;
+}
+
+static bool GetDescriptionSysV4L(int dev, std::string* desc) {
+  wpi::SmallString<64> ifpath;
+  {
+    wpi::raw_svector_ostream oss{ifpath};
+    oss << "/sys/class/video4linux/video" << dev << "/device/interface";
+  }
 
   int fd = open(ifpath.c_str(), O_RDONLY);
   if (fd < 0) return false;
@@ -192,23 +218,35 @@ static bool GetDescriptionIoctl(const char* cpath, std::string* desc) {
   return true;
 }
 
-static std::string GetDescriptionImpl(const char* cpath) {
+static int GetDeviceNum(const char* cpath) {
   wpi::StringRef path{cpath};
-  char pathBuf[128];
-  std::string rv;
+  std::string pathBuf;
 
-  // If trying to get by id or path, follow symlink
-  if (path.startswith("/dev/v4l/by-id/")) {
-    ssize_t n = readlink(cpath, pathBuf, sizeof(pathBuf));
-    if (n > 0) path = wpi::StringRef(pathBuf, n);
-  } else if (path.startswith("/dev/v4l/by-path/")) {
-    ssize_t n = readlink(cpath, pathBuf, sizeof(pathBuf));
-    if (n > 0) path = wpi::StringRef(pathBuf, n);
+  // it might be a symlink; if so, find the symlink target (e.g. /dev/videoN),
+  // add that to the list and make it the keypath
+  if (wpi::sys::fs::is_symlink_file(cpath)) {
+    char* target = ::realpath(cpath, nullptr);
+    if (target) {
+      pathBuf = target;
+      path = pathBuf;
+      std::free(target);
+    }
   }
 
-  if (path.startswith("/dev/video")) {
+  path = wpi::sys::path::filename(path);
+  if (!path.startswith("video")) return -1;
+  int dev = -1;
+  if (path.substr(5).getAsInteger(10, dev)) return -1;
+  return dev;
+}
+
+static std::string GetDescriptionImpl(const char* cpath) {
+  std::string rv;
+
+  int dev = GetDeviceNum(cpath);
+  if (dev >= 0) {
     // Sometimes the /sys tree gives a better name.
-    if (GetDescriptionSysV4L(path, &rv)) return rv;
+    if (GetDescriptionSysV4L(dev, &rv)) return rv;
   }
 
   // Otherwise use an ioctl to query the caps and get the card name
@@ -1154,8 +1192,14 @@ void UsbCameraImpl::SetQuirks() {
   wpi::StringRef desc = GetDescription(descbuf);
   m_lifecam_exposure =
       desc.endswith("LifeCam HD-3000") || desc.endswith("LifeCam Cinema (TM)");
-  m_ps3eyecam_exposure =
-      desc.endswith("Camera-B4.04.27.1") || desc.endswith("Camera-B4.09.24.1");
+
+  int deviceNum = GetDeviceNum(m_path.c_str());
+  if (deviceNum >= 0) {
+    int vendorId, productId;
+    if (GetVendorProduct(deviceNum, &vendorId, &productId)) {
+      m_ps3eyecam_exposure = vendorId == 0x2000 && productId == 0x0145;
+    }
+  }
 }
 
 void UsbCameraImpl::SetProperty(int property, int value, CS_Status* status) {
@@ -1318,24 +1362,15 @@ UsbCameraInfo GetUsbCameraInfo(CS_Source source, CS_Status* status) {
   std::string keypath = static_cast<UsbCameraImpl&>(*data->source).GetPath();
   info.path = keypath;
 
-  // it might be a symlink; if so, find the symlink target (e.g. /dev/videoN),
-  // add that to the list and make it the keypath
-  if (wpi::sys::fs::is_symlink_file(keypath)) {
-    char* target = ::realpath(keypath.c_str(), nullptr);
-    if (target) {
-      keypath.assign(target);
-      info.otherPaths.emplace_back(keypath);
-      std::free(target);
-    }
-  }
-
   // device number
-  wpi::StringRef fname = wpi::sys::path::filename(keypath);
-  if (fname.startswith("video")) fname.substr(5).getAsInteger(10, info.dev);
+  info.dev = GetDeviceNum(keypath.c_str());
 
   // description
   info.name = GetDescriptionImpl(keypath.c_str());
 
+  // vendor/product id
+  GetVendorProduct(info.dev, &info.vendorId, &info.productId);
+
   // look through /dev/v4l/by-id and /dev/v4l/by-path for symlinks to the
   // keypath
   wpi::SmallString<128> path;
@@ -1386,6 +1421,8 @@ std::vector<UsbCameraInfo> EnumerateUsbCameras(CS_Status* status) {
       info.name = GetDescriptionImpl(path.c_str());
       if (info.name.empty()) continue;
 
+      GetVendorProduct(dev, &info.vendorId, &info.productId);
+
       if (dev >= retval.size()) retval.resize(info.dev + 1);
       retval[info.dev] = std::move(info);
     }

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

@@ -385,8 +385,8 @@ void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLength,
   }
 
   digitalSystem->writePulseLength(
-      static_cast<uint8_t>(1.0e9 * pulseLength /
-                           (pwmSystem->readLoopTiming(status) * 25)),
+      static_cast<uint16_t>(1.0e9 * pulseLength /
+                            (pwmSystem->readLoopTiming(status) * 25)),
       status);
   digitalSystem->writePulse(pulse, status);
 }

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

@@ -30,64 +30,9 @@ struct HAL_JoystickAxesInt {
 
 static constexpr int kJoystickPorts = 6;
 
-// Joystick User Data
-static std::unique_ptr<HAL_JoystickAxes[]> m_joystickAxes;
-static std::unique_ptr<HAL_JoystickPOVs[]> m_joystickPOVs;
-static std::unique_ptr<HAL_JoystickButtons[]> m_joystickButtons;
-static std::unique_ptr<HAL_JoystickDescriptor[]> m_joystickDescriptor;
-static std::unique_ptr<HAL_MatchInfo> m_matchInfo;
-
-// Joystick Cached Data
-static std::unique_ptr<HAL_JoystickAxes[]> m_joystickAxesCache;
-static std::unique_ptr<HAL_JoystickPOVs[]> m_joystickPOVsCache;
-static std::unique_ptr<HAL_JoystickButtons[]> m_joystickButtonsCache;
-static std::unique_ptr<HAL_JoystickDescriptor[]> m_joystickDescriptorCache;
-static std::unique_ptr<HAL_MatchInfo> m_matchInfoCache;
-
-static wpi::mutex m_cacheDataMutex;
-
-// Control word variables
-static HAL_ControlWord m_controlWordCache;
-static std::chrono::steady_clock::time_point m_lastControlWordUpdate;
-static wpi::mutex m_controlWordMutex;
-
 // Message and Data variables
 static wpi::mutex msgMutex;
 
-static void InitializeDriverStationCaches() {
-  m_joystickAxes = std::make_unique<HAL_JoystickAxes[]>(kJoystickPorts);
-  m_joystickPOVs = std::make_unique<HAL_JoystickPOVs[]>(kJoystickPorts);
-  m_joystickButtons = std::make_unique<HAL_JoystickButtons[]>(kJoystickPorts);
-  m_joystickDescriptor =
-      std::make_unique<HAL_JoystickDescriptor[]>(kJoystickPorts);
-  m_matchInfo = std::make_unique<HAL_MatchInfo>();
-  m_joystickAxesCache = std::make_unique<HAL_JoystickAxes[]>(kJoystickPorts);
-  m_joystickPOVsCache = std::make_unique<HAL_JoystickPOVs[]>(kJoystickPorts);
-  m_joystickButtonsCache =
-      std::make_unique<HAL_JoystickButtons[]>(kJoystickPorts);
-  m_joystickDescriptorCache =
-      std::make_unique<HAL_JoystickDescriptor[]>(kJoystickPorts);
-  m_matchInfoCache = std::make_unique<HAL_MatchInfo>();
-
-  // All joysticks should default to having zero axes, povs and buttons, so
-  // uninitialized memory doesn't get sent to speed controllers.
-  for (unsigned int i = 0; i < kJoystickPorts; i++) {
-    m_joystickAxes[i].count = 0;
-    m_joystickPOVs[i].count = 0;
-    m_joystickButtons[i].count = 0;
-    m_joystickDescriptor[i].isXbox = 0;
-    m_joystickDescriptor[i].type = -1;
-    m_joystickDescriptor[i].name[0] = '\0';
-
-    m_joystickAxesCache[i].count = 0;
-    m_joystickPOVsCache[i].count = 0;
-    m_joystickButtonsCache[i].count = 0;
-    m_joystickDescriptorCache[i].isXbox = 0;
-    m_joystickDescriptorCache[i].type = -1;
-    m_joystickDescriptorCache[i].name[0] = '\0';
-  }
-}
-
 static int32_t HAL_GetJoystickAxesInternal(int32_t joystickNum,
                                            HAL_JoystickAxes* axes) {
   HAL_JoystickAxesInt axesInt;
@@ -177,90 +122,18 @@ static int32_t HAL_GetMatchInfoInternal(HAL_MatchInfo* info) {
   return status;
 }
 
-static void UpdateDriverStationControlWord(bool force,
-                                           HAL_ControlWord& controlWord) {
-  auto now = std::chrono::steady_clock::now();
-  std::scoped_lock lock(m_controlWordMutex);
-  // Update every 50 ms or on force.
-  if ((now - m_lastControlWordUpdate > std::chrono::milliseconds(50)) ||
-      force) {
-    HAL_GetControlWordInternal(&m_controlWordCache);
-    m_lastControlWordUpdate = now;
-  }
-  controlWord = m_controlWordCache;
-}
-
-static void UpdateDriverStationDataCaches() {
-  // Get the status of all of the joysticks, and save to the cache
-  for (uint8_t stick = 0; stick < kJoystickPorts; stick++) {
-    HAL_GetJoystickAxesInternal(stick, &m_joystickAxesCache[stick]);
-    HAL_GetJoystickPOVsInternal(stick, &m_joystickPOVsCache[stick]);
-    HAL_GetJoystickButtonsInternal(stick, &m_joystickButtonsCache[stick]);
-    HAL_GetJoystickDescriptorInternal(stick, &m_joystickDescriptorCache[stick]);
-  }
-  // Grab match specific data
-  HAL_GetMatchInfoInternal(m_matchInfoCache.get());
-
-  // Force a control word update, to make sure the data is the newest.
-  HAL_ControlWord controlWord;
-  UpdateDriverStationControlWord(true, controlWord);
-
-  {
-    // Obtain a lock on the data, swap the cached data into the main data arrays
-    std::scoped_lock lock(m_cacheDataMutex);
-
-    m_joystickAxes.swap(m_joystickAxesCache);
-    m_joystickPOVs.swap(m_joystickPOVsCache);
-    m_joystickButtons.swap(m_joystickButtonsCache);
-    m_joystickDescriptor.swap(m_joystickDescriptorCache);
-    m_matchInfo.swap(m_matchInfoCache);
-  }
-}
-
-class DriverStationThread : public wpi::SafeThread {
- public:
-  void Main() {
-    std::unique_lock lock(m_mutex);
-    while (m_active) {
-      m_cond.wait(lock, [&] { return !m_active || m_notify; });
-      if (!m_active) break;
-      m_notify = false;
-
-      lock.unlock();
-      UpdateDriverStationDataCaches();
-      lock.lock();
-
-      // Notify all threads
-      newDSDataAvailableCounter++;
-      newDSDataAvailableCond.notify_all();
-    }
-
-    // Notify waiters on thread exit
-    newDSDataAvailableCounter++;
-    newDSDataAvailableCond.notify_all();
-  }
-
-  bool m_notify = false;
-  wpi::condition_variable newDSDataAvailableCond;
-  int newDSDataAvailableCounter{0};
-};
-
-class DriverStationThreadOwner
-    : public wpi::SafeThreadOwner<DriverStationThread> {
- public:
-  void Notify() {
-    auto thr = GetThread();
-    if (!thr) return;
-    thr->m_notify = true;
-    thr->m_cond.notify_one();
-  }
-};
-
-static std::unique_ptr<DriverStationThreadOwner> dsThread = nullptr;
+static wpi::mutex* newDSDataAvailableMutex;
+static wpi::condition_variable* newDSDataAvailableCond;
+static int newDSDataAvailableCounter{0};
 
 namespace hal {
 namespace init {
-void InitializeFRCDriverStation() {}
+void InitializeFRCDriverStation() {
+  static wpi::mutex newMutex;
+  newDSDataAvailableMutex = &newMutex;
+  static wpi::condition_variable newCond;
+  newDSDataAvailableCond = &newCond;
+}
 }  // namespace init
 }  // namespace hal
 
@@ -357,41 +230,29 @@ int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
 }
 
 int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
-  std::memset(controlWord, 0, sizeof(HAL_ControlWord));
-  UpdateDriverStationControlWord(false, *controlWord);
-  return 0;
+  return HAL_GetControlWordInternal(controlWord);
 }
 
 int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
-  std::unique_lock lock(m_cacheDataMutex);
-  *axes = m_joystickAxes[joystickNum];
-  return 0;
+  return HAL_GetJoystickAxesInternal(joystickNum, axes);
 }
 
 int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) {
-  std::unique_lock lock(m_cacheDataMutex);
-  *povs = m_joystickPOVs[joystickNum];
-  return 0;
+  return HAL_GetJoystickPOVsInternal(joystickNum, povs);
 }
 
 int32_t HAL_GetJoystickButtons(int32_t joystickNum,
                                HAL_JoystickButtons* buttons) {
-  std::unique_lock lock(m_cacheDataMutex);
-  *buttons = m_joystickButtons[joystickNum];
-  return 0;
+  return HAL_GetJoystickButtonsInternal(joystickNum, buttons);
 }
 
 int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
                                   HAL_JoystickDescriptor* desc) {
-  std::unique_lock lock(m_cacheDataMutex);
-  *desc = m_joystickDescriptor[joystickNum];
-  return 0;
+  return HAL_GetJoystickDescriptorInternal(joystickNum, desc);
 }
 
 int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) {
-  std::unique_lock lock(m_cacheDataMutex);
-  *info = *m_matchInfo;
-  return 0;
+  return HAL_GetMatchInfoInternal(info);
 }
 
 HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
@@ -483,10 +344,8 @@ HAL_Bool HAL_IsNewControlData(void) {
   // 20ms rate occurs once every 2.7 years of DS connected runtime, so not
   // worth the cycles to check.
   thread_local int lastCount{-1};
-  if (!dsThread) return false;
-  auto thr = dsThread->GetThread();
-  if (!thr) return false;
-  int currentCount = thr->newDSDataAvailableCounter;
+  std::lock_guard lock{*newDSDataAvailableMutex};
+  int currentCount = newDSDataAvailableCounter;
   if (lastCount == currentCount) return false;
   lastCount = currentCount;
   return true;
@@ -506,19 +365,16 @@ HAL_Bool HAL_WaitForDSDataTimeout(double timeout) {
   auto timeoutTime =
       std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
 
-  if (!dsThread) return false;
-  auto thr = dsThread->GetThread();
-  if (!thr) return false;
-  int currentCount = thr->newDSDataAvailableCounter;
-  while (thr->newDSDataAvailableCounter == currentCount) {
+  std::unique_lock lock{*newDSDataAvailableMutex};
+  int currentCount = newDSDataAvailableCounter;
+  while (newDSDataAvailableCounter == currentCount) {
     if (timeout > 0) {
-      auto timedOut =
-          thr->newDSDataAvailableCond.wait_until(thr.GetLock(), timeoutTime);
+      auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime);
       if (timedOut == std::cv_status::timeout) {
         return false;
       }
     } else {
-      thr->newDSDataAvailableCond.wait(thr.GetLock());
+      newDSDataAvailableCond->wait(lock);
     }
   }
   return true;
@@ -531,7 +387,9 @@ static void newDataOccur(uint32_t refNum) {
   // Since we could get other values, require our specific handle
   // to signal our threads
   if (refNum != refNumber) return;
-  dsThread->Notify();
+  // Notify all threads
+  newDSDataAvailableCounter++;
+  newDSDataAvailableCond->notify_all();
 }
 
 /*
@@ -549,11 +407,6 @@ void HAL_InitializeDriverStation(void) {
   // Second check in case another thread was waiting
   if (initialized) return;
 
-  InitializeDriverStationCaches();
-
-  dsThread = std::make_unique<DriverStationThreadOwner>();
-  dsThread->Start();
-
   // Set up the occur function internally with NetComm
   NetCommRPCProxy_SetOccurFuncPointer(newDataOccur);
   // Set up our occur reference number

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

@@ -277,24 +277,6 @@ HAL_Bool HAL_GetBrownedOut(int32_t* status) {
   return !(watchdog->readStatus_PowerAlive(status));
 }
 
-void HAL_BaseInitialize(int32_t* status) {
-  static std::atomic_bool initialized{false};
-  static wpi::mutex initializeMutex;
-  // Initial check, as if it's true initialization has finished
-  if (initialized) return;
-
-  std::scoped_lock lock(initializeMutex);
-  // Second check in case another thread was waiting
-  if (initialized) return;
-  // image 4; Fixes errors caused by multiple processes. Talk to NI about this
-  nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass =
-      nLoadOut::kTargetClass_RoboRIO;
-
-  global.reset(tGlobal::create(status));
-  watchdog.reset(tSysWatchdog::create(status));
-  initialized = true;
-}
-
 static bool killExistingProgram(int timeout, int mode) {
   // Kill any previous robot programs
   std::fstream fs;
@@ -368,8 +350,14 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
     setNewDataSem(nullptr);
   });
 
+  // image 4; Fixes errors caused by multiple processes. Talk to NI about this
+  nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass =
+      nLoadOut::kTargetClass_RoboRIO;
+
   int32_t status = 0;
-  HAL_BaseInitialize(&status);
+  global.reset(tGlobal::create(&status));
+  watchdog.reset(tSysWatchdog::create(&status));
+
   if (status != 0) return false;
 
   HAL_InitializeDriverStation();

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

@@ -77,16 +77,6 @@ HAL_Bool HAL_GetSystemActive(int32_t* status);
  */
 HAL_Bool HAL_GetBrownedOut(int32_t* status);
 
-/**
- * The base HAL initialize function. Useful if you need to ensure the DS and
- * base HAL functions (the ones above this declaration in HAL.h) are properly
- * initialized. For normal programs and executables, please use HAL_Initialize.
- *
- * This is mainly expected to be use from libraries that are expected to be used
- * from LabVIEW, as it handles its own initialization for objects.
- */
-void HAL_BaseInitialize(int32_t* status);
-
 /**
  * Gets a port handle for a specific channel.
  *

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

@@ -37,6 +37,7 @@ SimDeviceData::Value* SimDeviceData::LookupValue(HAL_SimValueHandle handle) {
 
   // look up device
   Device* deviceImpl = LookupDevice(handle >> 16);
+  if (!deviceImpl) return nullptr;
 
   // look up value
   handle &= 0xffff;

imgui/CMakeLists.txt

@@ -14,6 +14,24 @@ if(result)
   message(FATAL_ERROR "Build step for imgui failed: ${result}")
 endif()
 
+# Build font
+add_executable(imgui_font_bin2c ${CMAKE_CURRENT_BINARY_DIR}/imgui-src/misc/fonts/binary_to_compressed_c.cpp)
+add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ProggyDotted.inc
+  COMMAND imgui_font_bin2c
+  ARGS "${CMAKE_CURRENT_BINARY_DIR}/proggyfonts-src/ProggyDotted/ProggyDotted Regular.ttf" ProggyDotted > ${CMAKE_CURRENT_BINARY_DIR}/ProggyDotted.inc
+  WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}
+  MAIN_DEPENDENCY "${CMAKE_CURRENT_BINARY_DIR}/proggyfonts-src/ProggyDotted/ProggyDotted Regular.ttf"
+  VERBATIM
+)
+file(GENERATE OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.cpp
+    CONTENT "#include \"imgui_ProggyDotted.h\"\n#include \"ProggyDotted.inc\"\nImFont* ImGui::AddFontProggyDotted(ImGuiIO& io, float size_pixels, const ImFontConfig* font_cfg, const ImWchar* glyph_ranges) {\n  return io.Fonts->AddFontFromMemoryCompressedTTF(ProggyDotted_compressed_data, ProggyDotted_compressed_size, size_pixels, font_cfg, glyph_ranges);\n}\n"
+)
+file(GENERATE OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.h
+    CONTENT "#pragma once\n#include \"imgui.h\"\nnamespace ImGui {\nImFont* AddFontProggyDotted(ImGuiIO& io, float size_pixels, const ImFontConfig* font_cfg = nullptr, const ImWchar* glyph_ranges = nullptr);\n}\n"
+)
+set_source_files_properties(${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.cpp
+    PROPERTIES OBJECT_DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/ProggyDotted.inc)
+
 # Add imgui directly to our build.
 set(SAVE_BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS})
 set(BUILD_SHARED_LIBS OFF)
@@ -28,8 +46,8 @@ add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/gl3w-src
 
 set(imgui_srcdir ${CMAKE_CURRENT_BINARY_DIR}/imgui-src)
 file(GLOB imgui_sources ${imgui_srcdir}/*.cpp)
-add_library(imgui STATIC ${imgui_sources} ${imgui_srcdir}/examples/imgui_impl_glfw.cpp ${imgui_srcdir}/examples/imgui_impl_opengl3.cpp)
+add_library(imgui STATIC ${imgui_sources} ${imgui_srcdir}/examples/imgui_impl_glfw.cpp ${imgui_srcdir}/examples/imgui_impl_opengl3.cpp ${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.cpp)
 target_link_libraries(imgui PUBLIC gl3w glfw)
-target_include_directories(imgui PUBLIC "$<BUILD_INTERFACE:${imgui_srcdir}>" "$<BUILD_INTERFACE:${imgui_srcdir}/examples>")
+target_include_directories(imgui PUBLIC "$<BUILD_INTERFACE:${imgui_srcdir}>" "$<BUILD_INTERFACE:${imgui_srcdir}/examples>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>")
 
 set_property(TARGET imgui PROPERTY POSITION_INDEPENDENT_CODE ON)

imgui/CMakeLists.txt.in

@@ -31,3 +31,13 @@ ExternalProject_Add(imgui
   INSTALL_COMMAND   ""
   TEST_COMMAND      ""
 )
+ExternalProject_Add(proggyfonts
+  GIT_REPOSITORY    https://github.com/bluescan/proggyfonts.git
+  GIT_TAG           v1.1.5
+  SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/proggyfonts-src"
+  BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/proggyfonts-build"
+  CONFIGURE_COMMAND ""
+  BUILD_COMMAND     ""
+  INSTALL_COMMAND   ""
+  TEST_COMMAND      ""
+)

myRobot/build.gradle

@@ -69,7 +69,10 @@ model {
                     lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
                     lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                     lib project: ':cscore', library: 'cscore', linkage: 'shared'
+                    lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
+                    lib project: ':cscore', library: 'cscoreJNIShared', linkage: 'shared'
                     project(':hal').addHalDependency(binary, 'shared')
+                    project(':hal').addHalJniDependency(binary)
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                     lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
             }

shared/config.gradle

@@ -8,10 +8,10 @@ nativeUtils {
   wpi {
     configureDependencies {
       wpiVersion = "-1"
-      niLibVersion = "2020.4.1"
-      opencvVersion = "3.4.7-1"
+      niLibVersion = "2020.5.1"
+      opencvVersion = "3.4.7-2"
       googleTestVersion = "1.9.0-3-437e100"
-      imguiVersion = "1.72b-1"
+      imguiVersion = "1.72b-2"
     }
   }
 }

shared/opencv.gradle

@@ -1,4 +1,4 @@
-def opencvVersion = '3.4.7-1'
+def opencvVersion = '3.4.7-2'
 
 if (project.hasProperty('useCpp') && project.useCpp) {
     model {

simulation/halsim_gazebo/src/main/native/cpp/GazeboAnalogIn.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,8 +10,8 @@
 #include <string>
 
 #include <hal/Power.h>
+#include <hal/Value.h>
 #include <mockdata/AnalogInData.h>
-#include <mockdata/HAL_Value.h>
 #include <mockdata/NotifyListener.h>
 
 static void init_callback(const char* name, void* param,

simulation/halsim_gazebo/src/main/native/cpp/GazeboDIO.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,8 +9,8 @@
 
 #include <string>
 
+#include <hal/Value.h>
 #include <mockdata/DIOData.h>
-#include <mockdata/HAL_Value.h>
 #include <mockdata/NotifyListener.h>
 
 static void init_callback(const char* name, void* param,

simulation/halsim_gazebo/src/main/native/cpp/GazeboEncoder.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,8 +9,8 @@
 
 #include <string>
 
+#include <hal/Value.h>
 #include <mockdata/EncoderData.h>
-#include <mockdata/HAL_Value.h>
 #include <mockdata/NotifyListener.h>
 
 static void encoder_init_callback(const char* name, void* param,
@@ -54,7 +54,7 @@ void GazeboEncoder::Control(const char* command) {
   if (!m_pub) {
     m_pub = m_halsim->node.Advertise<gazebo::msgs::String>(
         "~/simulator/encoder/dio/" +
-        std::to_string(HALSIM_GetDigitalChannelA(m_index)) + "/control");
+        std::to_string(HALSIM_GetEncoderDigitalChannelA(m_index)) + "/control");
     m_pub->WaitForConnection(gazebo::common::Time(1, 0));
   }
   gazebo::msgs::String msg;
@@ -66,7 +66,8 @@ void GazeboEncoder::Listen() {
   if (!m_sub)
     m_sub = m_halsim->node.Subscribe<gazebo::msgs::Float64>(
         "~/simulator/encoder/dio/" +
-            std::to_string(HALSIM_GetDigitalChannelA(m_index)) + "/position",
+            std::to_string(HALSIM_GetEncoderDigitalChannelA(m_index)) +
+            "/position",
         &GazeboEncoder::Callback, this);
 }
 

simulation/halsim_gazebo/src/main/native/cpp/GazeboPCM.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 
 #include <string>
 
-#include <mockdata/HAL_Value.h>
+#include <hal/Value.h>
 #include <mockdata/NotifyListener.h>
 #include <mockdata/PCMData.h>
 

simulation/halsim_gazebo/src/main/native/cpp/GazeboPWM.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 
 #include <string>
 
-#include <mockdata/HAL_Value.h>
+#include <hal/Value.h>
 #include <mockdata/NotifyListener.h>
 #include <mockdata/PWMData.h>
 

simulation/halsim_gui/build.gradle

@@ -23,6 +23,10 @@ if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxra
         binaries {
             all {
                 nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
+                    it.buildable = false
+                    return
+                }
                 if (it.targetPlatform.operatingSystem.isWindows()) {
                     it.linker.args << 'Gdi32.lib' << 'Shell32.lib'
                 } else if (it.targetPlatform.operatingSystem.isMacOsX()) {

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

@@ -14,6 +14,7 @@
 #include <imgui.h>
 #include <mockdata/PCMData.h>
 
+#include "HALSimGui.h"
 #include "SimDeviceGui.h"
 
 using namespace halsimgui;
@@ -28,7 +29,10 @@ static void DisplayCompressors() {
       HAL_Value value;
 
       // enabled
-      value = HAL_MakeBoolean(HALSIM_GetPCMCompressorOn(i));
+      if (HALSimGui::AreOutputsDisabled())
+        value = HAL_MakeBoolean(false);
+      else
+        value = HAL_MakeBoolean(HALSIM_GetPCMCompressorOn(i));
       if (SimDeviceGui::DisplayValue("Running", false, &value))
         HALSIM_SetPCMCompressorOn(i, value.data.v_boolean);
 

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

@@ -56,7 +56,7 @@ static void DisplayDIO() {
     }
   }
 
-  ImGui::PushItemWidth(100);
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
   for (int i = 0; i < numDIO; ++i) {
     if (HALSIM_GetDIOInitialized(i)) {
       hasAny = true;

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

@@ -211,7 +211,12 @@ void RobotJoystick::Update() {
   const unsigned char* sysButtons;
   if (sys->isGamepad && useGamepad) {
     sysAxes = sys->gamepadState.axes;
+    // don't remap on windows
+#ifdef _WIN32
+    sysButtons = sys->buttons;
+#else
     sysButtons = sys->gamepadState.buttons;
+#endif
   } else {
     sysAxes = sys->axes;
     sysButtons = sys->buttons;
@@ -226,13 +231,33 @@ void RobotJoystick::Update() {
   desc.buttonCount = (std::min)(sys->buttonCount, 32);
   desc.povCount = (std::min)(sys->hatCount, HAL_kMaxJoystickPOVs);
 
-  axes.count = desc.axisCount;
-  std::memcpy(axes.axes, sysAxes, axes.count * sizeof(&axes.axes[0]));
-
   buttons.count = desc.buttonCount;
   for (int j = 0; j < buttons.count; ++j)
     buttons.buttons |= (sysButtons[j] ? 1u : 0u) << j;
 
+  axes.count = desc.axisCount;
+  if (sys->isGamepad && useGamepad) {
+    // the FRC DriverStation maps gamepad (XInput) trigger values to 0-1 range
+    // on axis 2 and 3.
+    axes.axes[0] = sysAxes[0];
+    axes.axes[1] = sysAxes[1];
+    axes.axes[2] = 0.5 + sysAxes[4] / 2.0;
+    axes.axes[3] = 0.5 + sysAxes[5] / 2.0;
+    axes.axes[4] = sysAxes[2];
+    axes.axes[5] = sysAxes[3];
+
+    // the start button for gamepads is not mapped on the FRC DriverStation
+    // platforms, so remove it if present
+    if (buttons.count == 11) {
+      --desc.buttonCount;
+      --buttons.count;
+      buttons.buttons =
+          (buttons.buttons & 0xff) | ((buttons.buttons >> 1) & 0x300);
+    }
+  } else {
+    std::memcpy(axes.axes, sysAxes, axes.count * sizeof(&axes.axes[0]));
+  }
+
   povs.count = desc.povCount;
   for (int j = 0; j < povs.count; ++j) povs.povs[j] = HatToAngle(sys->hats[j]);
 }
@@ -326,7 +351,7 @@ static void DisplayFMS() {
   static const char* stations[] = {"Red 1",  "Red 2",  "Red 3",
                                    "Blue 1", "Blue 2", "Blue 3"};
   int allianceStationId = HALSIM_GetDriverStationAllianceStationId();
-  ImGui::SetNextItemWidth(100);
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
   if (ImGui::Combo("Alliance Station", &allianceStationId, stations, 6))
     HALSIM_SetDriverStationAllianceStationId(
         static_cast<HAL_AllianceStationID>(allianceStationId));
@@ -337,7 +362,7 @@ static void DisplayFMS() {
 
   static double startMatchTime = 0.0;
   double matchTime = HALSIM_GetDriverStationMatchTime();
-  ImGui::SetNextItemWidth(100);
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
   if (ImGui::InputDouble("Match Time", &matchTime, 0, 0, "%.1f",
                          ImGuiInputTextFlags_EnterReturnsTrue)) {
     HALSIM_SetDriverStationMatchTime(matchTime);
@@ -355,7 +380,7 @@ static void DisplayFMS() {
 
   // Game Specific Message
   static HAL_MatchInfo matchInfo;
-  ImGui::SetNextItemWidth(100);
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
   if (ImGui::InputText("Game Specific",
                        reinterpret_cast<char*>(matchInfo.gameSpecificMessage),
                        sizeof(matchInfo.gameSpecificMessage),
@@ -395,7 +420,7 @@ static void DisplaySystemJoysticks() {
 
 static void DisplayJoysticks() {
   // imgui doesn't size columns properly with autoresize, so force it
-  ImGui::Dummy(ImVec2(14.0 * 9 * HAL_kMaxJoysticks, 0));
+  ImGui::Dummy(ImVec2(ImGui::GetFontSize() * 10 * HAL_kMaxJoysticks, 0));
 
   ImGui::Columns(HAL_kMaxJoysticks, "Joysticks", false);
   for (int i = 0; i < HAL_kMaxJoysticks; ++i) {

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

@@ -54,7 +54,7 @@ static void EncodersWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
 static void DisplayEncoders() {
   bool hasAny = false;
   static int numEncoder = HAL_GetNumEncoders();
-  ImGui::PushItemWidth(100);
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
   for (int i = 0; i < numEncoder; ++i) {
     if (HALSIM_GetEncoderInitialized(i)) {
       hasAny = true;

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

@@ -12,6 +12,8 @@ namespace halsimgui {
 void DrawLEDs(int* values, int numValues, int cols, const ImU32* colors,
               float size, float spacing) {
   if (numValues == 0) return;
+  if (size == 0) size = ImGui::GetFontSize() / 2.0;
+  if (spacing == 0) spacing = ImGui::GetFontSize() / 3.0;
 
   ImDrawList* drawList = ImGui::GetWindowDrawList();
   const ImVec2 p = ImGui::GetCursorScreenPos();

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

@@ -13,9 +13,11 @@
 #include <GL/gl3w.h>
 #include <GLFW/glfw3.h>
 #include <imgui.h>
+#include <imgui_ProggyDotted.h>
 #include <imgui_impl_glfw.h>
 #include <imgui_impl_opengl3.h>
 #include <imgui_internal.h>
+#include <mockdata/DriverStationData.h>
 #include <wpi/StringMap.h>
 #include <wpi/raw_ostream.h>
 
@@ -43,6 +45,12 @@ struct WindowInfo {
 
 static std::atomic_bool gExit{false};
 static GLFWwindow* gWindow;
+static bool gWindowLoadedWidthHeight = false;
+static int gWindowWidth = 1280;
+static int gWindowHeight = 720;
+static int gWindowMaximized = 0;
+static int gWindowXPos = -1;
+static int gWindowYPos = -1;
 static std::vector<std::function<void()>> gInitializers;
 static std::vector<std::function<void()>> gExecutors;
 static std::vector<WindowInfo> gWindows;
@@ -50,15 +58,40 @@ static wpi::StringMap<int> gWindowMap;   // index into gWindows
 static std::vector<int> gSortedWindows;  // index into gWindows
 static std::vector<std::function<void()>> gOptionMenus;
 static std::vector<std::function<void()>> gMenus;
+static int gUserScale = 2;
+static int gStyle = 0;
+static constexpr int kScaledFontLevels = 9;
+static ImFont* gScaledFont[kScaledFontLevels];
+static bool gDisableOutputsOnDSDisable = true;
 
 static void glfw_error_callback(int error, const char* description) {
   wpi::errs() << "GLFW Error " << error << ": " << description << '\n';
 }
 
+static void glfw_window_size_callback(GLFWwindow*, int width, int height) {
+  if (!gWindowMaximized) {
+    gWindowWidth = width;
+    gWindowHeight = height;
+  }
+}
+
+static void glfw_window_maximize_callback(GLFWwindow* window, int maximized) {
+  gWindowMaximized = maximized;
+}
+
+static void glfw_window_pos_callback(GLFWwindow* window, int xpos, int ypos) {
+  if (!gWindowMaximized) {
+    gWindowXPos = xpos;
+    gWindowYPos = ypos;
+  }
+}
+
 // read/write open state to ini file
 static void* SimWindowsReadOpen(ImGuiContext* ctx,
                                 ImGuiSettingsHandler* handler,
                                 const char* name) {
+  if (wpi::StringRef{name} == "GLOBAL") return &gWindow;
+
   int index = gWindowMap.try_emplace(name, gWindows.size()).first->second;
   if (index == static_cast<int>(gWindows.size())) {
     gSortedWindows.push_back(index);
@@ -71,11 +104,37 @@ static void* SimWindowsReadOpen(ImGuiContext* ctx,
 
 static void SimWindowsReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
                                void* entry, const char* lineStr) {
-  auto element = static_cast<WindowInfo*>(entry);
   wpi::StringRef line{lineStr};
   auto [name, value] = line.split('=');
   name = name.trim();
   value = value.trim();
+
+  if (entry == &gWindow) {
+    int num;
+    if (value.getAsInteger(10, num)) return;
+    if (name == "width") {
+      gWindowWidth = num;
+      gWindowLoadedWidthHeight = true;
+    } else if (name == "height") {
+      gWindowHeight = num;
+      gWindowLoadedWidthHeight = true;
+    } else if (name == "maximized") {
+      gWindowMaximized = num;
+    } else if (name == "xpos") {
+      gWindowXPos = num;
+    } else if (name == "ypos") {
+      gWindowYPos = num;
+    } else if (name == "userScale") {
+      gUserScale = num;
+    } else if (name == "style") {
+      gStyle = num;
+    } else if (name == "disableOutputsOnDS") {
+      gDisableOutputsOnDSDisable = num;
+    }
+    return;
+  }
+
+  auto element = static_cast<WindowInfo*>(entry);
   if (name == "visible") {
     int num;
     if (value.getAsInteger(10, num)) return;
@@ -89,12 +148,31 @@ static void SimWindowsReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
 
 static void SimWindowsWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
                                ImGuiTextBuffer* out_buf) {
+  out_buf->appendf(
+      "[SimWindow][GLOBAL]\nwidth=%d\nheight=%d\nmaximized=%d\n"
+      "xpos=%d\nypos=%d\nuserScale=%d\nstyle=%d\ndisableOutputsOnDS=%d\n",
+      gWindowWidth, gWindowHeight, gWindowMaximized, gWindowXPos, gWindowYPos,
+      gUserScale, gStyle, gDisableOutputsOnDSDisable ? 1 : 0);
   for (auto&& window : gWindows)
     out_buf->appendf("[SimWindow][%s]\nvisible=%d\nenabled=%d\n\n",
                      window.name.c_str(), window.visible ? 1 : 0,
                      window.enabled ? 1 : 0);
 }
 
+static void UpdateStyle() {
+  switch (gStyle) {
+    case 0:
+      ImGui::StyleColorsClassic();
+      break;
+    case 1:
+      ImGui::StyleColorsDark();
+      break;
+    case 2:
+      ImGui::StyleColorsLight();
+      break;
+  }
+}
+
 void HALSimGui::Add(std::function<void()> initialize) {
   if (initialize) gInitializers.emplace_back(std::move(initialize));
 }
@@ -170,6 +248,10 @@ void HALSimGui::SetDefaultWindowSize(const char* name, float width,
   window.size = ImVec2{width, height};
 }
 
+bool HALSimGui::AreOutputsDisabled() {
+  return gDisableOutputsOnDSDisable && !HALSIM_GetDriverStationEnabled();
+}
+
 bool HALSimGui::Initialize() {
   // Setup window
   glfwSetErrorCallback(glfw_error_callback);
@@ -184,6 +266,7 @@ bool HALSimGui::Initialize() {
   glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 2);
   glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);  // 3.2+ only
   glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);  // Required on Mac
+  glfwWindowHint(GLFW_COCOA_GRAPHICS_SWITCHING, GLFW_TRUE);
 #else
   // GL 3.0 + GLSL 130
   const char* glsl_version = "#version 130";
@@ -193,10 +276,86 @@ bool HALSimGui::Initialize() {
   // glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE); // 3.0+
 #endif
 
+  // Setup Dear ImGui context
+  IMGUI_CHECKVERSION();
+  ImGui::CreateContext();
+  ImGuiIO& io = ImGui::GetIO();
+  (void)io;
+
+  // Hook ini handler to save settings
+  ImGuiSettingsHandler iniHandler;
+  iniHandler.TypeName = "SimWindow";
+  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
+  iniHandler.ReadOpenFn = SimWindowsReadOpen;
+  iniHandler.ReadLineFn = SimWindowsReadLine;
+  iniHandler.WriteAllFn = SimWindowsWriteAll;
+  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
+
+  for (auto&& initialize : gInitializers) {
+    if (initialize) initialize();
+  }
+
+  // Load INI file
+  ImGui::LoadIniSettingsFromDisk(io.IniFilename);
+
+  // Set initial window settings
+  glfwWindowHint(GLFW_MAXIMIZED, gWindowMaximized ? GLFW_TRUE : GLFW_FALSE);
+
+  float windowScale = 1.0;
+  if (!gWindowLoadedWidthHeight) {
+    glfwWindowHint(GLFW_SCALE_TO_MONITOR, GLFW_TRUE);
+    // get the primary monitor work area to see if we have a reasonable initial
+    // window size; if not, maximize, and default scaling to smaller
+    if (GLFWmonitor* primary = glfwGetPrimaryMonitor()) {
+      int monWidth, monHeight;
+      glfwGetMonitorWorkarea(primary, nullptr, nullptr, &monWidth, &monHeight);
+      if (monWidth < gWindowWidth || monHeight < gWindowHeight) {
+        glfwWindowHint(GLFW_MAXIMIZED, GLFW_TRUE);
+        windowScale = (std::min)(monWidth * 1.0 / gWindowWidth,
+                                 monHeight * 1.0 / gWindowHeight);
+      }
+    }
+  }
+  if (gWindowXPos != -1 && gWindowYPos != -1)
+    glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE);
+
   // Create window with graphics context
-  gWindow =
-      glfwCreateWindow(1280, 720, "Robot Simulation GUI", nullptr, nullptr);
+  gWindow = glfwCreateWindow(gWindowWidth, gWindowHeight,
+                             "Robot Simulation GUI", nullptr, nullptr);
   if (!gWindow) return false;
+
+  if (!gWindowLoadedWidthHeight) {
+    if (windowScale == 1.0)
+      glfwGetWindowContentScale(gWindow, &windowScale, nullptr);
+    wpi::outs() << "windowScale = " << windowScale;
+    // force user scale if window scale is smaller
+    if (windowScale <= 0.5)
+      gUserScale = 0;
+    else if (windowScale <= 0.75)
+      gUserScale = 1;
+    if (windowScale != 1.0) {
+      // scale default window positions
+      for (auto&& window : gWindows) {
+        if ((window.posCond & ImGuiCond_FirstUseEver) != 0) {
+          window.pos.x *= windowScale;
+          window.pos.y *= windowScale;
+        }
+      }
+    }
+  }
+
+  // Update window settings
+  if (gWindowXPos != -1 && gWindowYPos != -1) {
+    glfwSetWindowPos(gWindow, gWindowXPos, gWindowYPos);
+    glfwShowWindow(gWindow);
+  }
+
+  // Set window callbacks
+  glfwGetWindowSize(gWindow, &gWindowWidth, &gWindowHeight);
+  glfwSetWindowSizeCallback(gWindow, glfw_window_size_callback);
+  glfwSetWindowMaximizeCallback(gWindow, glfw_window_maximize_callback);
+  glfwSetWindowPosCallback(gWindow, glfw_window_pos_callback);
+
   glfwMakeContextCurrent(gWindow);
   glfwSwapInterval(1);  // Enable vsync
 
@@ -206,15 +365,8 @@ bool HALSimGui::Initialize() {
     return false;
   }
 
-  // Setup Dear ImGui context
-  IMGUI_CHECKVERSION();
-  ImGui::CreateContext();
-  ImGuiIO& io = ImGui::GetIO();
-  (void)io;
-
   // Setup Dear ImGui style
-  // ImGui::StyleColorsDark();
-  ImGui::StyleColorsClassic();
+  UpdateStyle();
 
   // Setup Platform/Renderer bindings
   ImGui_ImplGlfw_InitForOpenGL(gWindow, true);
@@ -244,17 +396,14 @@ bool HALSimGui::Initialize() {
   // io.Fonts->AddFontFromFileTTF("c:\\Windows\\Fonts\\ArialUni.ttf", 18.0f,
   // NULL, io.Fonts->GetGlyphRangesJapanese()); IM_ASSERT(font != NULL);
 
-  // hook ini handler to save settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = "SimWindow";
-  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
-  iniHandler.ReadOpenFn = SimWindowsReadOpen;
-  iniHandler.ReadLineFn = SimWindowsReadLine;
-  iniHandler.WriteAllFn = SimWindowsWriteAll;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-
-  for (auto&& initialize : gInitializers) {
-    if (initialize) initialize();
+  // this range is based on 13px being the "nominal" 100% size and going from
+  // ~0.5x (7px) to ~2.0x (25px)
+  for (int i = 0; i < kScaledFontLevels; ++i) {
+    float size = 7.0f + i * 3.0f;
+    ImFontConfig cfg;
+    std::snprintf(cfg.Name, sizeof(cfg.Name), "ProggyDotted-%d",
+                  static_cast<int>(size));
+    gScaledFont[i] = ImGui::AddFontProggyDotted(io, size, &cfg);
   }
 
   return true;
@@ -273,6 +422,16 @@ void HALSimGui::Main(void*) {
     ImGui_ImplGlfw_NewFrame();
     ImGui::NewFrame();
 
+    // Scale based on OS window content scaling
+    float windowScale = 1.0;
+    glfwGetWindowContentScale(gWindow, &windowScale, nullptr);
+    // map to closest font size: 0 = 0.5x, 1 = 0.75x, 2 = 1.0x, 3 = 1.25x,
+    // 4 = 1.5x, 5 = 1.75x, 6 = 2x
+    int fontScale =
+        std::clamp(gUserScale + static_cast<int>((windowScale - 1.0) * 4), 0,
+                   kScaledFontLevels - 1);
+    ImGui::GetIO().FontDefault = gScaledFont[fontScale];
+
     for (auto&& execute : gExecutors) {
       if (execute) execute();
     }
@@ -281,12 +440,50 @@ void HALSimGui::Main(void*) {
       ImGui::BeginMainMenuBar();
 
       if (ImGui::BeginMenu("Options")) {
+        ImGui::MenuItem("Disable outputs on DS disable", nullptr,
+                        &gDisableOutputsOnDSDisable, true);
         for (auto&& menu : gOptionMenus) {
           if (menu) menu();
         }
         ImGui::EndMenu();
       }
 
+      if (ImGui::BeginMenu("View")) {
+        if (ImGui::BeginMenu("Style")) {
+          bool selected;
+          selected = gStyle == 0;
+          if (ImGui::MenuItem("Classic", nullptr, &selected, true)) {
+            gStyle = 0;
+            UpdateStyle();
+          }
+          selected = gStyle == 1;
+          if (ImGui::MenuItem("Dark", nullptr, &selected, true)) {
+            gStyle = 1;
+            UpdateStyle();
+          }
+          selected = gStyle == 2;
+          if (ImGui::MenuItem("Light", nullptr, &selected, true)) {
+            gStyle = 2;
+            UpdateStyle();
+          }
+          ImGui::EndMenu();
+        }
+
+        if (ImGui::BeginMenu("Zoom")) {
+          for (int i = 0; i < kScaledFontLevels && (25 * (i + 2)) <= 200; ++i) {
+            char label[20];
+            std::snprintf(label, sizeof(label), "%d%%", 25 * (i + 2));
+            bool selected = gUserScale == i;
+            bool enabled = (fontScale - gUserScale + i) >= 0 &&
+                           (fontScale - gUserScale + i) < kScaledFontLevels;
+            if (ImGui::MenuItem(label, nullptr, &selected, enabled))
+              gUserScale = i;
+          }
+          ImGui::EndMenu();
+        }
+        ImGui::EndMenu();
+      }
+
       if (ImGui::BeginMenu("Window")) {
         for (auto&& windowIndex : gSortedWindows) {
           auto& window = gWindows[windowIndex];
@@ -394,4 +591,8 @@ void HALSIMGUI_SetDefaultWindowSize(const char* name, float width,
   HALSimGui::SetDefaultWindowSize(name, width, height);
 }
 
+int HALSIMGUI_AreOutputsDisabled(void) {
+  return HALSimGui::AreOutputsDisabled();
+}
+
 }  // extern "C"

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

@@ -21,7 +21,7 @@ static void DisplayPDP() {
   bool hasAny = false;
   static int numPDP = HAL_GetNumPDPModules();
   static int numChannels = HAL_GetNumPDPChannels();
-  ImGui::PushItemWidth(150);
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 13);
   for (int i = 0; i < numPDP; ++i) {
     if (HALSIM_GetPDPInitialized(i)) {
       hasAny = true;

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

@@ -42,7 +42,7 @@ static void DisplayPWMs() {
 
       char name[32];
       std::snprintf(name, sizeof(name), "PWM[%d]", i);
-      float val = HALSIM_GetPWMSpeed(i);
+      float val = HALSimGui::AreOutputsDisabled() ? 0 : HALSIM_GetPWMSpeed(i);
       ImGui::Value(name, val, "%0.3f");
 
       // lazily build history storage

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

@@ -35,8 +35,12 @@ static void DisplayRelays() {
       else
         first = false;
 
-      bool forward = HALSIM_GetRelayForward(i);
-      bool reverse = HALSIM_GetRelayReverse(i);
+      bool forward = false;
+      bool reverse = false;
+      if (!HALSimGui::AreOutputsDisabled()) {
+        reverse = HALSIM_GetRelayReverse(i);
+        forward = HALSIM_GetRelayForward(i);
+      }
 
       ImGui::Text("Relay[%d]", i);
       ImGui::SameLine();

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

@@ -18,7 +18,7 @@ static void DisplayRoboRio() {
   ImGui::Button("User Button");
   HALSIM_SetRoboRioFPGAButton(0, ImGui::IsItemActive());
 
-  ImGui::PushItemWidth(100);
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
 
   if (ImGui::CollapsingHeader("RoboRIO Input")) {
     {

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

@@ -31,7 +31,10 @@ static void DisplaySolenoids() {
     for (int j = 0; j < numChannels; ++j) {
       if (HALSIM_GetPCMSolenoidInitialized(i, j)) {
         anyInit = true;
-        channels[j] = HALSIM_GetPCMSolenoidOutput(i, j) ? 1 : -1;
+        channels[j] = (!HALSimGui::AreOutputsDisabled() &&
+                       HALSIM_GetPCMSolenoidOutput(i, j))
+                          ? 1
+                          : -1;
       } else {
         channels[j] = -2;
       }

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

@@ -23,10 +23,12 @@ namespace halsimgui {
  * @param numValues size of values array
  * @param cols number of columns
  * @param colors colors array
- * @param size size of each LED (both horizontal and vertical)
- * @param spacing spacing between each LED (both horizontal and vertical)
+ * @param size size of each LED (both horizontal and vertical);
+ *             if 0, defaults to 1/2 of font size
+ * @param spacing spacing between each LED (both horizontal and vertical);
+ *                if 0, defaults to 1/3 of font size
  */
 void DrawLEDs(int* values, int numValues, int cols, const ImU32* colors,
-              float size = 8.0f, float spacing = 6.0f);
+              float size = 0.0f, float spacing = 0.0f);
 
 }  // namespace halsimgui

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

@@ -23,6 +23,7 @@ void HALSIMGUI_SetWindowVisibility(const char* name, int32_t visibility);
 void HALSIMGUI_SetDefaultWindowPos(const char* name, float x, float y);
 void HALSIMGUI_SetDefaultWindowSize(const char* name, float width,
                                     float height);
+int HALSIMGUI_AreOutputsDisabled(void);
 
 }  // extern "C"
 
@@ -125,6 +126,13 @@ class HALSimGui {
    * @param height height
    */
   static void SetDefaultWindowSize(const char* name, float width, float height);
+
+  /**
+   * Returns true if outputs are disabled.
+   *
+   * @return true if outputs are disabled, false otherwise.
+   */
+  static bool AreOutputsDisabled();
 };
 
 }  // namespace halsimgui

wpilibc/CMakeLists.txt

@@ -8,7 +8,7 @@ find_package( OpenCV REQUIRED )
 configure_file(src/generate/WPILibVersion.cpp.in WPILibVersion.cpp)
 
 file(GLOB_RECURSE
-    wpilibc_native_src src/main/native/cpp/*.cpp)
+    wpilibc_native_src src/main/native/cpp/*.cpp src/main/native/cppcs/*.cpp)
 
 add_library(wpilibc ${wpilibc_native_src} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
 set_target_properties(wpilibc PROPERTIES DEBUG_POSTFIX "d")

wpilibc/build.gradle

@@ -116,31 +116,39 @@ model {
                     it.buildable = false
                     return
                 }
+                cppCompiler.define 'DYNAMIC_CAMERA_SERVER'
                 lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
-                lib project: ':cscore', library: 'cscore', linkage: 'shared'
                 project(':hal').addHalDependency(it, 'shared')
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-                lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
             }
         }
         "${nativeName}"(NativeLibrarySpec) {
             sources {
                 cpp {
                     source {
-                        srcDirs "${rootDir}/shared/singlelib"
+                        srcDirs "src/main/native/cppcs"
                         include '**/*.cpp'
                     }
                     exportedHeaders {
-                        srcDirs 'src/main/native/include'
+                        srcDirs 'src/main/native/include', '../cameraserver/src/main/native/include'
                     }
                 }
             }
             binaries.all {
                 lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
-                lib project: ':cscore', library: 'cscore', linkage: 'shared'
                 project(':hal').addHalDependency(it, 'shared')
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-                lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+
+                if (it instanceof SharedLibraryBinarySpec) {
+                    cppCompiler.define 'DYNAMIC_CAMERA_SERVER'
+                    if (buildType == buildTypes.debug) {
+                        cppCompiler.define 'DYNAMIC_CAMERA_SERVER_DEBUG'
+                    }
+                } else {
+                    lib project: ':cscore', library: 'cscore', linkage: 'shared'
+                    lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+                }
+
             }
             appendDebugPathToBinaries(binaries)
         }

wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp

@@ -19,15 +19,15 @@ static constexpr auto kSamplePeriod = 0.0005_s;
 static constexpr double kCalibrationSampleTime = 5.0;
 static constexpr double kDegreePerSecondPerLSB = 0.0125;
 
-static constexpr int kRateRegister = 0x00;
-static constexpr int kTemRegister = 0x02;
-static constexpr int kLoCSTRegister = 0x04;
-static constexpr int kHiCSTRegister = 0x06;
-static constexpr int kQuadRegister = 0x08;
-static constexpr int kFaultRegister = 0x0A;
+// static constexpr int kRateRegister = 0x00;
+// static constexpr int kTemRegister = 0x02;
+// static constexpr int kLoCSTRegister = 0x04;
+// static constexpr int kHiCSTRegister = 0x06;
+// static constexpr int kQuadRegister = 0x08;
+// static constexpr int kFaultRegister = 0x0A;
 static constexpr int kPIDRegister = 0x0C;
-static constexpr int kSNHighRegister = 0x0E;
-static constexpr int kSNLowRegister = 0x10;
+// static constexpr int kSNHighRegister = 0x0E;
+// static constexpr int kSNLowRegister = 0x10;
 
 ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {}
 

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,7 +14,7 @@
 
 using namespace frc;
 
-GenericHID::GenericHID(int port) : m_ds(DriverStation::GetInstance()) {
+GenericHID::GenericHID(int port) : m_ds(&DriverStation::GetInstance()) {
   if (port >= DriverStation::kJoystickPorts) {
     wpi_setWPIError(BadJoystickIndex);
   }
@@ -22,39 +22,41 @@ GenericHID::GenericHID(int port) : m_ds(DriverStation::GetInstance()) {
 }
 
 bool GenericHID::GetRawButton(int button) const {
-  return m_ds.GetStickButton(m_port, button);
+  return m_ds->GetStickButton(m_port, button);
 }
 
 bool GenericHID::GetRawButtonPressed(int button) {
-  return m_ds.GetStickButtonPressed(m_port, button);
+  return m_ds->GetStickButtonPressed(m_port, button);
 }
 
 bool GenericHID::GetRawButtonReleased(int button) {
-  return m_ds.GetStickButtonReleased(m_port, button);
+  return m_ds->GetStickButtonReleased(m_port, button);
 }
 
 double GenericHID::GetRawAxis(int axis) const {
-  return m_ds.GetStickAxis(m_port, axis);
+  return m_ds->GetStickAxis(m_port, axis);
 }
 
-int GenericHID::GetPOV(int pov) const { return m_ds.GetStickPOV(m_port, pov); }
+int GenericHID::GetPOV(int pov) const { return m_ds->GetStickPOV(m_port, pov); }
 
-int GenericHID::GetAxisCount() const { return m_ds.GetStickAxisCount(m_port); }
+int GenericHID::GetAxisCount() const { return m_ds->GetStickAxisCount(m_port); }
 
-int GenericHID::GetPOVCount() const { return m_ds.GetStickPOVCount(m_port); }
+int GenericHID::GetPOVCount() const { return m_ds->GetStickPOVCount(m_port); }
 
 int GenericHID::GetButtonCount() const {
-  return m_ds.GetStickButtonCount(m_port);
+  return m_ds->GetStickButtonCount(m_port);
 }
 
 GenericHID::HIDType GenericHID::GetType() const {
-  return static_cast<HIDType>(m_ds.GetJoystickType(m_port));
+  return static_cast<HIDType>(m_ds->GetJoystickType(m_port));
 }
 
-std::string GenericHID::GetName() const { return m_ds.GetJoystickName(m_port); }
+std::string GenericHID::GetName() const {
+  return m_ds->GetJoystickName(m_port);
+}
 
 int GenericHID::GetAxisType(int axis) const {
-  return m_ds.GetJoystickAxisType(m_port, axis);
+  return m_ds->GetJoystickAxisType(m_port, axis);
 }
 
 int GenericHID::GetPort() const { return m_port; }

wpilibc/src/main/native/cpp/Timer.cpp

@@ -17,108 +17,34 @@
 
 namespace frc {
 
-void Wait(double seconds) {
-  std::this_thread::sleep_for(std::chrono::duration<double>(seconds));
-}
+void Wait(double seconds) { frc2::Wait(units::second_t(seconds)); }
 
-double GetTime() {
-  using std::chrono::duration;
-  using std::chrono::duration_cast;
-  using std::chrono::system_clock;
-
-  return duration_cast<duration<double>>(system_clock::now().time_since_epoch())
-      .count();
-}
+double GetTime() { return frc2::GetTime().to<double>(); }
 
 }  // namespace frc
 
 using namespace frc;
 
-// for compatibility with msvc12--see C2864
-const double Timer::kRolloverTime = (1ll << 32) / 1e6;
+const double Timer::kRolloverTime = frc2::Timer::kRolloverTime.to<double>();
 
 Timer::Timer() { Reset(); }
 
-Timer::Timer(Timer&& rhs)
-    : m_startTime(std::move(rhs.m_startTime)),
-      m_accumulatedTime(std::move(rhs.m_accumulatedTime)),
-      m_running(std::move(rhs.m_running)) {}
+double Timer::Get() const { return m_timer.Get().to<double>(); }
 
-Timer& Timer::operator=(Timer&& rhs) {
-  std::scoped_lock lock(m_mutex, rhs.m_mutex);
+void Timer::Reset() { m_timer.Reset(); }
 
-  m_startTime = std::move(rhs.m_startTime);
-  m_accumulatedTime = std::move(rhs.m_accumulatedTime);
-  m_running = std::move(rhs.m_running);
+void Timer::Start() { m_timer.Start(); }
 
-  return *this;
-}
-
-double Timer::Get() const {
-  double result;
-  double currentTime = GetFPGATimestamp();
-
-  std::scoped_lock lock(m_mutex);
-  if (m_running) {
-    // If the current time is before the start time, then the FPGA clock rolled
-    // over. Compensate by adding the ~71 minutes that it takes to roll over to
-    // the current time.
-    if (currentTime < m_startTime) {
-      currentTime += kRolloverTime;
-    }
-
-    result = (currentTime - m_startTime) + m_accumulatedTime;
-  } else {
-    result = m_accumulatedTime;
-  }
-
-  return result;
-}
-
-void Timer::Reset() {
-  std::scoped_lock lock(m_mutex);
-  m_accumulatedTime = 0;
-  m_startTime = GetFPGATimestamp();
-}
-
-void Timer::Start() {
-  std::scoped_lock lock(m_mutex);
-  if (!m_running) {
-    m_startTime = GetFPGATimestamp();
-    m_running = true;
-  }
-}
-
-void Timer::Stop() {
-  double temp = Get();
-
-  std::scoped_lock lock(m_mutex);
-  if (m_running) {
-    m_accumulatedTime = temp;
-    m_running = false;
-  }
-}
+void Timer::Stop() { m_timer.Stop(); }
 
 bool Timer::HasPeriodPassed(double period) {
-  return HasPeriodPassed(units::second_t(period));
-}
-
-bool Timer::HasPeriodPassed(units::second_t period) {
-  if (Get() > period.to<double>()) {
-    std::scoped_lock lock(m_mutex);
-    // Advance the start time by the period.
-    m_startTime += period.to<double>();
-    // Don't set it to the current time... we want to avoid drift.
-    return true;
-  }
-  return false;
+  return m_timer.HasPeriodPassed(units::second_t(period));
 }
 
 double Timer::GetFPGATimestamp() {
-  // FPGA returns the timestamp in microseconds
-  return RobotController::GetFPGATime() * 1.0e-6;
+  return frc2::Timer::GetFPGATimestamp().to<double>();
 }
 
 double Timer::GetMatchTime() {
-  return DriverStation::GetInstance().GetMatchTime();
+  return frc2::Timer::GetMatchTime().to<double>();
 }

wpilibc/src/main/native/cpp/buttons/Trigger.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,6 +15,27 @@
 
 using namespace frc;
 
+Trigger::Trigger(const Trigger& rhs) : SendableHelper(rhs) {}
+
+Trigger& Trigger::operator=(const Trigger& rhs) {
+  SendableHelper::operator=(rhs);
+  m_sendablePressed = false;
+  return *this;
+}
+
+Trigger::Trigger(Trigger&& rhs)
+    : SendableHelper(std::move(rhs)),
+      m_sendablePressed(rhs.m_sendablePressed.load()) {
+  rhs.m_sendablePressed = false;
+}
+
+Trigger& Trigger::operator=(Trigger&& rhs) {
+  SendableHelper::operator=(std::move(rhs));
+  m_sendablePressed = rhs.m_sendablePressed.load();
+  rhs.m_sendablePressed = false;
+  return *this;
+}
+
 bool Trigger::Grab() { return Get() || m_sendablePressed; }
 
 void Trigger::WhenActive(Command* command) {

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

@@ -35,12 +35,16 @@ units::second_t ProfiledPIDController::GetPeriod() const {
   return m_controller.GetPeriod();
 }
 
+void ProfiledPIDController::SetGoal(TrapezoidProfile::State goal) {
+  m_goal = goal;
+}
+
 void ProfiledPIDController::SetGoal(units::meter_t goal) {
   m_goal = {goal, 0_mps};
 }
 
-units::meter_t ProfiledPIDController::GetGoal() const {
-  return m_goal.position;
+TrapezoidProfile::State ProfiledPIDController::GetGoal() const {
+  return m_goal;
 }
 
 bool ProfiledPIDController::AtGoal() const {
@@ -52,8 +56,8 @@ void ProfiledPIDController::SetConstraints(
   m_constraints = constraints;
 }
 
-double ProfiledPIDController::GetSetpoint() const {
-  return m_controller.GetSetpoint();
+TrapezoidProfile::State ProfiledPIDController::GetSetpoint() const {
+  return m_setpoint;
 }
 
 bool ProfiledPIDController::AtSetpoint() const {
@@ -87,20 +91,27 @@ double ProfiledPIDController::GetVelocityError() const {
   return m_controller.GetVelocityError();
 }
 
-double ProfiledPIDController::Calculate(double measurement) {
+double ProfiledPIDController::Calculate(units::meter_t measurement) {
   frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
   m_setpoint = profile.Calculate(GetPeriod());
-  return m_controller.Calculate(measurement, m_setpoint.position.to<double>());
+  return m_controller.Calculate(measurement.to<double>(),
+                                m_setpoint.position.to<double>());
 }
 
-double ProfiledPIDController::Calculate(double measurement,
+double ProfiledPIDController::Calculate(units::meter_t measurement,
+                                        TrapezoidProfile::State goal) {
+  SetGoal(goal);
+  return Calculate(measurement);
+}
+
+double ProfiledPIDController::Calculate(units::meter_t measurement,
                                         units::meter_t goal) {
   SetGoal(goal);
   return Calculate(measurement);
 }
 
 double ProfiledPIDController::Calculate(
-    double measurement, units::meter_t goal,
+    units::meter_t measurement, units::meter_t goal,
     frc::TrapezoidProfile::Constraints constraints) {
   SetConstraints(constraints);
   return Calculate(measurement, goal);
@@ -117,6 +128,6 @@ void ProfiledPIDController::InitSendable(frc::SendableBuilder& builder) {
   builder.AddDoubleProperty("d", [this] { return GetD(); },
                             [this](double value) { SetD(value); });
   builder.AddDoubleProperty(
-      "goal", [this] { return GetGoal().to<double>(); },
+      "goal", [this] { return GetGoal().position.to<double>(); },
       [this](double value) { SetGoal(units::meter_t{value}); });
 }

wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp

@@ -20,10 +20,10 @@ using namespace frc;
 
 DifferentialDrive::DifferentialDrive(SpeedController& leftMotor,
                                      SpeedController& rightMotor)
-    : m_leftMotor(leftMotor), m_rightMotor(rightMotor) {
+    : m_leftMotor(&leftMotor), m_rightMotor(&rightMotor) {
   auto& registry = SendableRegistry::GetInstance();
-  registry.AddChild(this, &m_leftMotor);
-  registry.AddChild(this, &m_rightMotor);
+  registry.AddChild(this, m_leftMotor);
+  registry.AddChild(this, m_rightMotor);
   static int instances = 0;
   ++instances;
   registry.AddLW(this, "DifferentialDrive", instances);
@@ -77,9 +77,9 @@ void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
     }
   }
 
-  m_leftMotor.Set(std::clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
+  m_leftMotor->Set(std::clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
   double maxOutput = m_maxOutput * m_rightSideInvertMultiplier;
-  m_rightMotor.Set(std::clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
+  m_rightMotor->Set(std::clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
 
   Feed();
 }
@@ -151,9 +151,9 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
     rightMotorOutput /= maxMagnitude;
   }
 
-  m_leftMotor.Set(leftMotorOutput * m_maxOutput);
-  m_rightMotor.Set(rightMotorOutput * m_maxOutput *
-                   m_rightSideInvertMultiplier);
+  m_leftMotor->Set(leftMotorOutput * m_maxOutput);
+  m_rightMotor->Set(rightMotorOutput * m_maxOutput *
+                    m_rightSideInvertMultiplier);
 
   Feed();
 }
@@ -180,8 +180,8 @@ void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
     rightSpeed = std::copysign(rightSpeed * rightSpeed, rightSpeed);
   }
 
-  m_leftMotor.Set(leftSpeed * m_maxOutput);
-  m_rightMotor.Set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
+  m_leftMotor->Set(leftSpeed * m_maxOutput);
+  m_rightMotor->Set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
 
   Feed();
 }
@@ -203,8 +203,8 @@ void DifferentialDrive::SetRightSideInverted(bool rightSideInverted) {
 }
 
 void DifferentialDrive::StopMotor() {
-  m_leftMotor.StopMotor();
-  m_rightMotor.StopMotor();
+  m_leftMotor->StopMotor();
+  m_rightMotor->StopMotor();
   Feed();
 }
 
@@ -217,12 +217,12 @@ void DifferentialDrive::InitSendable(SendableBuilder& builder) {
   builder.SetActuator(true);
   builder.SetSafeState([=] { StopMotor(); });
   builder.AddDoubleProperty("Left Motor Speed",
-                            [=]() { return m_leftMotor.Get(); },
-                            [=](double value) { m_leftMotor.Set(value); });
+                            [=]() { return m_leftMotor->Get(); },
+                            [=](double value) { m_leftMotor->Set(value); });
   builder.AddDoubleProperty(
       "Right Motor Speed",
-      [=]() { return m_rightMotor.Get() * m_rightSideInvertMultiplier; },
+      [=]() { return m_rightMotor->Get() * m_rightSideInvertMultiplier; },
       [=](double value) {
-        m_rightMotor.Set(value * m_rightSideInvertMultiplier);
+        m_rightMotor->Set(value * m_rightSideInvertMultiplier);
       });
 }

wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp

@@ -29,7 +29,9 @@ KilloughDrive::KilloughDrive(SpeedController& leftMotor,
                              SpeedController& rightMotor,
                              SpeedController& backMotor, double leftMotorAngle,
                              double rightMotorAngle, double backMotorAngle)
-    : m_leftMotor(leftMotor), m_rightMotor(rightMotor), m_backMotor(backMotor) {
+    : m_leftMotor(&leftMotor),
+      m_rightMotor(&rightMotor),
+      m_backMotor(&backMotor) {
   m_leftVec = {std::cos(leftMotorAngle * (wpi::math::pi / 180.0)),
                std::sin(leftMotorAngle * (wpi::math::pi / 180.0))};
   m_rightVec = {std::cos(rightMotorAngle * (wpi::math::pi / 180.0)),
@@ -37,9 +39,9 @@ KilloughDrive::KilloughDrive(SpeedController& leftMotor,
   m_backVec = {std::cos(backMotorAngle * (wpi::math::pi / 180.0)),
                std::sin(backMotorAngle * (wpi::math::pi / 180.0))};
   auto& registry = SendableRegistry::GetInstance();
-  registry.AddChild(this, &m_leftMotor);
-  registry.AddChild(this, &m_rightMotor);
-  registry.AddChild(this, &m_backMotor);
+  registry.AddChild(this, m_leftMotor);
+  registry.AddChild(this, m_rightMotor);
+  registry.AddChild(this, m_backMotor);
   static int instances = 0;
   ++instances;
   registry.AddLW(this, "KilloughDrive", instances);
@@ -70,9 +72,9 @@ void KilloughDrive::DriveCartesian(double ySpeed, double xSpeed,
 
   Normalize(wheelSpeeds);
 
-  m_leftMotor.Set(wheelSpeeds[kLeft] * m_maxOutput);
-  m_rightMotor.Set(wheelSpeeds[kRight] * m_maxOutput);
-  m_backMotor.Set(wheelSpeeds[kBack] * m_maxOutput);
+  m_leftMotor->Set(wheelSpeeds[kLeft] * m_maxOutput);
+  m_rightMotor->Set(wheelSpeeds[kRight] * m_maxOutput);
+  m_backMotor->Set(wheelSpeeds[kBack] * m_maxOutput);
 
   Feed();
 }
@@ -91,9 +93,9 @@ void KilloughDrive::DrivePolar(double magnitude, double angle,
 }
 
 void KilloughDrive::StopMotor() {
-  m_leftMotor.StopMotor();
-  m_rightMotor.StopMotor();
-  m_backMotor.StopMotor();
+  m_leftMotor->StopMotor();
+  m_rightMotor->StopMotor();
+  m_backMotor->StopMotor();
   Feed();
 }
 
@@ -106,12 +108,12 @@ void KilloughDrive::InitSendable(SendableBuilder& builder) {
   builder.SetActuator(true);
   builder.SetSafeState([=] { StopMotor(); });
   builder.AddDoubleProperty("Left Motor Speed",
-                            [=]() { return m_leftMotor.Get(); },
-                            [=](double value) { m_leftMotor.Set(value); });
+                            [=]() { return m_leftMotor->Get(); },
+                            [=](double value) { m_leftMotor->Set(value); });
   builder.AddDoubleProperty("Right Motor Speed",
-                            [=]() { return m_rightMotor.Get(); },
-                            [=](double value) { m_rightMotor.Set(value); });
+                            [=]() { return m_rightMotor->Get(); },
+                            [=](double value) { m_rightMotor->Set(value); });
   builder.AddDoubleProperty("Back Motor Speed",
-                            [=]() { return m_backMotor.Get(); },
-                            [=](double value) { m_backMotor.Set(value); });
+                            [=]() { return m_backMotor->Get(); },
+                            [=](double value) { m_backMotor->Set(value); });
 }

wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp

@@ -24,15 +24,15 @@ MecanumDrive::MecanumDrive(SpeedController& frontLeftMotor,
                            SpeedController& rearLeftMotor,
                            SpeedController& frontRightMotor,
                            SpeedController& rearRightMotor)
-    : m_frontLeftMotor(frontLeftMotor),
-      m_rearLeftMotor(rearLeftMotor),
-      m_frontRightMotor(frontRightMotor),
-      m_rearRightMotor(rearRightMotor) {
+    : m_frontLeftMotor(&frontLeftMotor),
+      m_rearLeftMotor(&rearLeftMotor),
+      m_frontRightMotor(&frontRightMotor),
+      m_rearRightMotor(&rearRightMotor) {
   auto& registry = SendableRegistry::GetInstance();
-  registry.AddChild(this, &m_frontLeftMotor);
-  registry.AddChild(this, &m_rearLeftMotor);
-  registry.AddChild(this, &m_frontRightMotor);
-  registry.AddChild(this, &m_rearRightMotor);
+  registry.AddChild(this, m_frontLeftMotor);
+  registry.AddChild(this, m_rearLeftMotor);
+  registry.AddChild(this, m_frontRightMotor);
+  registry.AddChild(this, m_rearRightMotor);
   static int instances = 0;
   ++instances;
   registry.AddLW(this, "MecanumDrive", instances);
@@ -64,12 +64,12 @@ void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed,
 
   Normalize(wheelSpeeds);
 
-  m_frontLeftMotor.Set(wheelSpeeds[kFrontLeft] * m_maxOutput);
-  m_frontRightMotor.Set(wheelSpeeds[kFrontRight] * m_maxOutput *
+  m_frontLeftMotor->Set(wheelSpeeds[kFrontLeft] * m_maxOutput);
+  m_frontRightMotor->Set(wheelSpeeds[kFrontRight] * m_maxOutput *
+                         m_rightSideInvertMultiplier);
+  m_rearLeftMotor->Set(wheelSpeeds[kRearLeft] * m_maxOutput);
+  m_rearRightMotor->Set(wheelSpeeds[kRearRight] * m_maxOutput *
                         m_rightSideInvertMultiplier);
-  m_rearLeftMotor.Set(wheelSpeeds[kRearLeft] * m_maxOutput);
-  m_rearRightMotor.Set(wheelSpeeds[kRearRight] * m_maxOutput *
-                       m_rightSideInvertMultiplier);
 
   Feed();
 }
@@ -96,10 +96,10 @@ void MecanumDrive::SetRightSideInverted(bool rightSideInverted) {
 }
 
 void MecanumDrive::StopMotor() {
-  m_frontLeftMotor.StopMotor();
-  m_frontRightMotor.StopMotor();
-  m_rearLeftMotor.StopMotor();
-  m_rearRightMotor.StopMotor();
+  m_frontLeftMotor->StopMotor();
+  m_frontRightMotor->StopMotor();
+  m_rearLeftMotor->StopMotor();
+  m_rearRightMotor->StopMotor();
   Feed();
 }
 
@@ -111,22 +111,22 @@ void MecanumDrive::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("MecanumDrive");
   builder.SetActuator(true);
   builder.SetSafeState([=] { StopMotor(); });
-  builder.AddDoubleProperty("Front Left Motor Speed",
-                            [=]() { return m_frontLeftMotor.Get(); },
-                            [=](double value) { m_frontLeftMotor.Set(value); });
+  builder.AddDoubleProperty(
+      "Front Left Motor Speed", [=]() { return m_frontLeftMotor->Get(); },
+      [=](double value) { m_frontLeftMotor->Set(value); });
   builder.AddDoubleProperty(
       "Front Right Motor Speed",
-      [=]() { return m_frontRightMotor.Get() * m_rightSideInvertMultiplier; },
+      [=]() { return m_frontRightMotor->Get() * m_rightSideInvertMultiplier; },
       [=](double value) {
-        m_frontRightMotor.Set(value * m_rightSideInvertMultiplier);
+        m_frontRightMotor->Set(value * m_rightSideInvertMultiplier);
       });
   builder.AddDoubleProperty("Rear Left Motor Speed",
-                            [=]() { return m_rearLeftMotor.Get(); },
-                            [=](double value) { m_rearLeftMotor.Set(value); });
+                            [=]() { return m_rearLeftMotor->Get(); },
+                            [=](double value) { m_rearLeftMotor->Set(value); });
   builder.AddDoubleProperty(
       "Rear Right Motor Speed",
-      [=]() { return m_rearRightMotor.Get() * m_rightSideInvertMultiplier; },
+      [=]() { return m_rearRightMotor->Get() * m_rightSideInvertMultiplier; },
       [=](double value) {
-        m_rearRightMotor.Set(value * m_rightSideInvertMultiplier);
+        m_rearRightMotor->Set(value * m_rightSideInvertMultiplier);
       });
 }

wpilibc/src/main/native/cpp/frc2/Timer.cpp

@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/Timer.h"
+
+#include <chrono>
+#include <thread>
+
+#include <hal/HAL.h>
+
+#include "frc/DriverStation.h"
+#include "frc/RobotController.h"
+
+namespace frc2 {
+
+void Wait(units::second_t seconds) {
+  std::this_thread::sleep_for(
+      std::chrono::duration<double>(seconds.to<double>()));
+}
+
+units::second_t GetTime() {
+  using std::chrono::duration;
+  using std::chrono::duration_cast;
+  using std::chrono::system_clock;
+
+  return units::second_t(
+      duration_cast<duration<double>>(system_clock::now().time_since_epoch())
+          .count());
+}
+
+}  // namespace frc2
+
+using namespace frc2;
+
+// for compatibility with msvc12--see C2864
+const units::second_t Timer::kRolloverTime = units::second_t((1ll << 32) / 1e6);
+
+Timer::Timer() { Reset(); }
+
+Timer::Timer(const Timer& rhs)
+    : m_startTime(rhs.m_startTime),
+      m_accumulatedTime(rhs.m_accumulatedTime),
+      m_running(rhs.m_running) {}
+
+Timer& Timer::operator=(const Timer& rhs) {
+  std::scoped_lock lock(m_mutex, rhs.m_mutex);
+
+  m_startTime = rhs.m_startTime;
+  m_accumulatedTime = rhs.m_accumulatedTime;
+  m_running = rhs.m_running;
+
+  return *this;
+}
+
+Timer::Timer(Timer&& rhs)
+    : m_startTime(std::move(rhs.m_startTime)),
+      m_accumulatedTime(std::move(rhs.m_accumulatedTime)),
+      m_running(std::move(rhs.m_running)) {}
+
+Timer& Timer::operator=(Timer&& rhs) {
+  std::scoped_lock lock(m_mutex, rhs.m_mutex);
+
+  m_startTime = std::move(rhs.m_startTime);
+  m_accumulatedTime = std::move(rhs.m_accumulatedTime);
+  m_running = std::move(rhs.m_running);
+
+  return *this;
+}
+
+units::second_t Timer::Get() const {
+  units::second_t result;
+  units::second_t currentTime = GetFPGATimestamp();
+
+  std::scoped_lock lock(m_mutex);
+  if (m_running) {
+    // If the current time is before the start time, then the FPGA clock rolled
+    // over. Compensate by adding the ~71 minutes that it takes to roll over to
+    // the current time.
+    if (currentTime < m_startTime) {
+      currentTime += kRolloverTime;
+    }
+
+    result = (currentTime - m_startTime) + m_accumulatedTime;
+  } else {
+    result = m_accumulatedTime;
+  }
+
+  return result;
+}
+
+void Timer::Reset() {
+  std::scoped_lock lock(m_mutex);
+  m_accumulatedTime = 0_s;
+  m_startTime = GetFPGATimestamp();
+}
+
+void Timer::Start() {
+  std::scoped_lock lock(m_mutex);
+  if (!m_running) {
+    m_startTime = GetFPGATimestamp();
+    m_running = true;
+  }
+}
+
+void Timer::Stop() {
+  units::second_t temp = Get();
+
+  std::scoped_lock lock(m_mutex);
+  if (m_running) {
+    m_accumulatedTime = temp;
+    m_running = false;
+  }
+}
+
+bool Timer::HasPeriodPassed(units::second_t period) {
+  if (Get() > period) {
+    std::scoped_lock lock(m_mutex);
+    // Advance the start time by the period.
+    m_startTime += period;
+    // Don't set it to the current time... we want to avoid drift.
+    return true;
+  }
+  return false;
+}
+
+units::second_t Timer::GetFPGATimestamp() {
+  // FPGA returns the timestamp in microseconds
+  return units::second_t(frc::RobotController::GetFPGATime()) * 1.0e-6;
+}
+
+units::second_t Timer::GetMatchTime() {
+  return units::second_t(frc::DriverStation::GetInstance().GetMatchTime());
+}

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

@@ -43,7 +43,7 @@ ParallelRaceGroup Command::WithTimeout(units::second_t duration) && {
   return ParallelRaceGroup(std::move(temp));
 }
 
-ParallelRaceGroup Command::InterruptOn(std::function<bool()> condition) && {
+ParallelRaceGroup Command::WithInterrupt(std::function<bool()> condition) && {
   std::vector<std::unique_ptr<Command>> temp;
   temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
   temp.emplace_back(std::move(*this).TransferOwnership());
@@ -58,7 +58,7 @@ SequentialCommandGroup Command::BeforeStarting(std::function<void()> toRun) && {
   return SequentialCommandGroup(std::move(temp));
 }
 
-SequentialCommandGroup Command::WhenFinished(std::function<void()> toRun) && {
+SequentialCommandGroup Command::AndThen(std::function<void()> toRun) && {
   std::vector<std::unique_ptr<Command>> temp;
   temp.emplace_back(std::move(*this).TransferOwnership());
   temp.emplace_back(std::make_unique<InstantCommand>(

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

@@ -39,6 +39,11 @@ void CommandScheduler::AddButton(wpi::unique_function<void()> button) {
 void CommandScheduler::ClearButtons() { m_buttons.clear(); }
 
 void CommandScheduler::Schedule(bool interruptible, Command* command) {
+  if (m_inRunLoop) {
+    m_toSchedule.try_emplace(command, interruptible);
+    return;
+  }
+
   if (command->IsGrouped()) {
     wpi_setWPIErrorWithContext(CommandIllegalUse,
                                "A command that is part of a command group "
@@ -125,6 +130,7 @@ void CommandScheduler::Run() {
     button();
   }
 
+  m_inRunLoop = true;
   // Run scheduled commands, remove finished commands.
   for (auto iterator = m_scheduledCommands.begin();
        iterator != m_scheduledCommands.end(); iterator++) {
@@ -153,6 +159,18 @@ void CommandScheduler::Run() {
       m_scheduledCommands.erase(iterator);
     }
   }
+  m_inRunLoop = false;
+
+  for (auto&& commandInterruptible : m_toSchedule) {
+    Schedule(commandInterruptible.second, commandInterruptible.first);
+  }
+
+  for (auto&& command : m_toCancel) {
+    Cancel(command);
+  }
+
+  m_toSchedule.clear();
+  m_toCancel.clear();
 
   // Add default commands for un-required registered subsystems.
   for (auto&& subsystem : m_subsystems) {
@@ -198,6 +216,11 @@ Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
 }
 
 void CommandScheduler::Cancel(Command* command) {
+  if (m_inRunLoop) {
+    m_toCancel.emplace_back(command);
+    return;
+  }
+
   auto find = m_scheduledCommands.find(command);
   if (find == m_scheduledCommands.end()) return;
   command->End(true);

wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp

@@ -26,16 +26,13 @@ void ParallelRaceGroup::Execute() {
     commandRunning->Execute();
     if (commandRunning->IsFinished()) {
       m_finished = true;
-      commandRunning->End(false);
     }
   }
 }
 
 void ParallelRaceGroup::End(bool interrupted) {
   for (auto& commandRunning : m_commands) {
-    if (!commandRunning->IsFinished()) {
-      commandRunning->End(true);
-    }
+    commandRunning->End(!commandRunning->IsFinished());
   }
   isRunning = false;
 }

wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ProfiledPIDCommand.h"
+
+using namespace frc2;
+using State = frc::TrapezoidProfile::State;
+
+ProfiledPIDCommand::ProfiledPIDCommand(
+    frc::ProfiledPIDController controller,
+    std::function<units::meter_t()> measurementSource,
+    std::function<State()> goalSource,
+    std::function<void(double, State)> useOutput,
+    std::initializer_list<Subsystem*> requirements)
+    : m_controller{controller},
+      m_measurement{std::move(measurementSource)},
+      m_goal{std::move(goalSource)},
+      m_useOutput{std::move(useOutput)} {
+  AddRequirements(requirements);
+}
+
+ProfiledPIDCommand::ProfiledPIDCommand(
+    frc::ProfiledPIDController controller,
+    std::function<units::meter_t()> measurementSource,
+    std::function<units::meter_t()> goalSource,
+    std::function<void(double, State)> useOutput,
+    std::initializer_list<Subsystem*> requirements)
+    : ProfiledPIDCommand(controller, measurementSource,
+                         [&goalSource]() {
+                           return State{goalSource(), 0_mps};
+                         },
+                         useOutput, requirements) {}
+
+ProfiledPIDCommand::ProfiledPIDCommand(
+    frc::ProfiledPIDController controller,
+    std::function<units::meter_t()> measurementSource, State goal,
+    std::function<void(double, State)> useOutput,
+    std::initializer_list<Subsystem*> requirements)
+    : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
+                         useOutput, requirements) {}
+
+ProfiledPIDCommand::ProfiledPIDCommand(
+    frc::ProfiledPIDController controller,
+    std::function<units::meter_t()> measurementSource, units::meter_t goal,
+    std::function<void(double, State)> useOutput,
+    std::initializer_list<Subsystem*> requirements)
+    : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
+                         useOutput, requirements) {}
+
+void ProfiledPIDCommand::Initialize() { m_controller.Reset(); }
+
+void ProfiledPIDCommand::Execute() {
+  UseOutput(m_controller.Calculate(GetMeasurement(), m_goal()),
+            m_controller.GetSetpoint());
+}
+
+void ProfiledPIDCommand::End(bool interrupted) {
+  UseOutput(0, State{0_m, 0_mps});
+}
+
+units::meter_t ProfiledPIDCommand::GetMeasurement() { return m_measurement(); }
+
+void ProfiledPIDCommand::UseOutput(double output, State state) {
+  m_useOutput(output, state);
+}
+
+frc::ProfiledPIDController& ProfiledPIDCommand::GetController() {
+  return m_controller;
+}

wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ProfiledPIDSubsystem.h"
+
+using namespace frc2;
+using State = frc::TrapezoidProfile::State;
+
+ProfiledPIDSubsystem::ProfiledPIDSubsystem(
+    frc::ProfiledPIDController controller)
+    : m_controller{controller} {}
+
+void ProfiledPIDSubsystem::Periodic() {
+  if (m_enabled) {
+    UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()),
+              m_controller.GetSetpoint());
+  }
+}
+
+void ProfiledPIDSubsystem::Enable() {
+  m_controller.Reset();
+  m_enabled = true;
+}
+
+void ProfiledPIDSubsystem::Disable() {
+  UseOutput(0, State{0_m, 0_mps});
+  m_enabled = false;
+}
+
+frc::ProfiledPIDController& ProfiledPIDSubsystem::GetController() {
+  return m_controller;
+}

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

@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/RamseteCommand.h"
+
+using namespace frc2;
+using namespace units;
+
+template <typename T>
+int sgn(T val) {
+  return (T(0) < val) - (val < T(0));
+}
+
+RamseteCommand::RamseteCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::RamseteController controller, volt_t ks,
+    units::unit_t<voltsecondspermeter> kv,
+    units::unit_t<voltsecondssquaredpermeter> ka,
+    frc::DifferentialDriveKinematics kinematics,
+    std::function<units::meters_per_second_t()> leftSpeed,
+    std::function<units::meters_per_second_t()> rightSpeed,
+    frc2::PIDController leftController, frc2::PIDController rightController,
+    std::function<void(volt_t, volt_t)> output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_controller(controller),
+      m_ks(ks),
+      m_kv(kv),
+      m_ka(ka),
+      m_kinematics(kinematics),
+      m_leftSpeed(leftSpeed),
+      m_rightSpeed(rightSpeed),
+      m_leftController(std::make_unique<frc2::PIDController>(leftController)),
+      m_rightController(std::make_unique<frc2::PIDController>(rightController)),
+      m_outputVolts(output) {
+  AddRequirements(requirements);
+}
+
+RamseteCommand::RamseteCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::RamseteController controller,
+    frc::DifferentialDriveKinematics kinematics,
+    std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
+        output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_controller(controller),
+      m_ks(0),
+      m_kv(0),
+      m_ka(0),
+      m_kinematics(kinematics),
+      m_outputVel(output) {
+  AddRequirements(requirements);
+}
+
+void RamseteCommand::Initialize() {
+  m_prevTime = 0_s;
+  auto initialState = m_trajectory.Sample(0_s);
+  m_prevSpeeds = m_kinematics.ToWheelSpeeds(
+      frc::ChassisSpeeds{initialState.velocity, 0_mps,
+                         initialState.velocity * initialState.curvature});
+  m_timer.Reset();
+  m_timer.Start();
+  m_leftController->Reset();
+  m_rightController->Reset();
+}
+
+void RamseteCommand::Execute() {
+  auto curTime = m_timer.Get();
+  auto dt = curTime - m_prevTime;
+
+  auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(
+      m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
+
+  if (m_leftController.get() != nullptr) {
+    auto leftFeedforward =
+        m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left +
+        m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt;
+
+    auto rightFeedforward =
+        m_ks * sgn(targetWheelSpeeds.right) + m_kv * targetWheelSpeeds.right +
+        m_ka * (targetWheelSpeeds.right - m_prevSpeeds.right) / dt;
+
+    auto leftOutput =
+        volt_t(m_leftController->Calculate(
+            m_leftSpeed().to<double>(), targetWheelSpeeds.left.to<double>())) +
+        leftFeedforward;
+
+    auto rightOutput = volt_t(m_rightController->Calculate(
+                           m_rightSpeed().to<double>(),
+                           targetWheelSpeeds.right.to<double>())) +
+                       rightFeedforward;
+
+    m_outputVolts(leftOutput, rightOutput);
+  } else {
+    m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right);
+  }
+
+  m_prevTime = curTime;
+  m_prevSpeeds = targetWheelSpeeds;
+}
+
+void RamseteCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool RamseteCommand::IsFinished() {
+  return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+}

wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp

@@ -38,7 +38,9 @@ void SequentialCommandGroup::Execute() {
 }
 
 void SequentialCommandGroup::End(bool interrupted) {
-  if (interrupted && !m_commands.empty()) {
+  if (interrupted && !m_commands.empty() &&
+      m_currentCommandIndex != invalid_index &&
+      m_currentCommandIndex < m_commands.size()) {
     m_commands[m_currentCommandIndex]->End(interrupted);
   }
   m_currentCommandIndex = invalid_index;

wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/TrapezoidProfileCommand.h"
+
+#include <units/units.h>
+
+using namespace frc2;
+
+TrapezoidProfileCommand::TrapezoidProfileCommand(
+    frc::TrapezoidProfile profile,
+    std::function<void(frc::TrapezoidProfile::State)> output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_profile(profile), m_output(output) {
+  AddRequirements(requirements);
+}
+
+void TrapezoidProfileCommand::Initialize() {
+  m_timer.Reset();
+  m_timer.Start();
+}
+
+void TrapezoidProfileCommand::Execute() {
+  m_output(m_profile.Calculate(m_timer.Get()));
+}
+
+void TrapezoidProfileCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool TrapezoidProfileCommand::IsFinished() {
+  return m_timer.HasPeriodPassed(m_profile.TotalTime());
+}

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

@@ -24,7 +24,6 @@ struct LiveWindow::Impl {
   Impl();
 
   struct Component {
-    SendableBuilderImpl builder;
     bool firstTime = true;
     bool telemetryEnabled = true;
   };
@@ -85,13 +84,11 @@ void LiveWindow::DisableTelemetry(Sendable* sendable) {
 void LiveWindow::DisableAllTelemetry() {
   std::scoped_lock lock(m_impl->mutex);
   m_impl->telemetryEnabled = false;
-  m_impl->registry.ForeachLiveWindow(
-      m_impl->dataHandle, [&](Sendable*, wpi::StringRef, wpi::StringRef,
-                              Sendable*, std::shared_ptr<void>& data) {
-        if (!data) data = std::make_shared<Impl::Component>();
-        std::static_pointer_cast<Impl::Component>(data)->telemetryEnabled =
-            false;
-      });
+  m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
+    if (!cbdata.data) cbdata.data = std::make_shared<Impl::Component>();
+    std::static_pointer_cast<Impl::Component>(cbdata.data)->telemetryEnabled =
+        false;
+  });
 }
 
 bool LiveWindow::IsEnabled() const {
@@ -111,13 +108,9 @@ void LiveWindow::SetEnabled(bool enabled) {
     scheduler->SetEnabled(false);
     scheduler->RemoveAll();
   } else {
-    m_impl->registry.ForeachLiveWindow(
-        m_impl->dataHandle, [&](Sendable*, wpi::StringRef, wpi::StringRef,
-                                Sendable*, std::shared_ptr<void>& data) {
-          if (data)
-            std::static_pointer_cast<Impl::Component>(data)
-                ->builder.StopLiveWindowMode();
-        });
+    m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
+      cbdata.builder.StopLiveWindowMode();
+    });
     scheduler->SetEnabled(true);
   }
   m_impl->enabledEntry.SetBoolean(enabled);
@@ -132,42 +125,39 @@ void LiveWindow::UpdateValuesUnsafe() {
   // Only do this if either LiveWindow mode or telemetry is enabled.
   if (!m_impl->liveWindowEnabled && !m_impl->telemetryEnabled) return;
 
-  m_impl->registry.ForeachLiveWindow(
-      m_impl->dataHandle,
-      [&](Sendable* sendable, wpi::StringRef name, wpi::StringRef subsystem,
-          Sendable* parent, std::shared_ptr<void>& data) {
-        if (!sendable || parent) return;
+  m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
+    if (!cbdata.sendable || cbdata.parent) return;
 
-        if (!data) data = std::make_shared<Impl::Component>();
+    if (!cbdata.data) cbdata.data = std::make_shared<Impl::Component>();
 
-        auto& comp = *std::static_pointer_cast<Impl::Component>(data);
+    auto& comp = *std::static_pointer_cast<Impl::Component>(cbdata.data);
 
-        if (!m_impl->liveWindowEnabled && !comp.telemetryEnabled) return;
+    if (!m_impl->liveWindowEnabled && !comp.telemetryEnabled) return;
 
-        if (comp.firstTime) {
-          // By holding off creating the NetworkTable entries, it allows the
-          // components to be redefined. This allows default sensor and actuator
-          // values to be created that are replaced with the custom names from
-          // users calling setName.
-          if (name.empty()) return;
-          auto ssTable = m_impl->liveWindowTable->GetSubTable(subsystem);
-          std::shared_ptr<NetworkTable> table;
-          // Treat name==subsystem as top level of subsystem
-          if (name == subsystem)
-            table = ssTable;
-          else
-            table = ssTable->GetSubTable(name);
-          table->GetEntry(".name").SetString(name);
-          comp.builder.SetTable(table);
-          sendable->InitSendable(comp.builder);
-          ssTable->GetEntry(".type").SetString("LW Subsystem");
+    if (comp.firstTime) {
+      // By holding off creating the NetworkTable entries, it allows the
+      // components to be redefined. This allows default sensor and actuator
+      // values to be created that are replaced with the custom names from
+      // users calling setName.
+      if (cbdata.name.empty()) return;
+      auto ssTable = m_impl->liveWindowTable->GetSubTable(cbdata.subsystem);
+      std::shared_ptr<NetworkTable> table;
+      // Treat name==subsystem as top level of subsystem
+      if (cbdata.name == cbdata.subsystem)
+        table = ssTable;
+      else
+        table = ssTable->GetSubTable(cbdata.name);
+      table->GetEntry(".name").SetString(cbdata.name);
+      cbdata.builder.SetTable(table);
+      cbdata.sendable->InitSendable(cbdata.builder);
+      ssTable->GetEntry(".type").SetString("LW Subsystem");
 
-          comp.firstTime = false;
-        }
+      comp.firstTime = false;
+    }
 
-        if (m_impl->startLiveWindow) comp.builder.StartLiveWindowMode();
-        comp.builder.UpdateTable();
-      });
+    if (m_impl->startLiveWindow) cbdata.builder.StartLiveWindowMode();
+    cbdata.builder.UpdateTable();
+  });
 
   m_impl->startLiveWindow = false;
 }

wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,41 +7,30 @@
 
 #include "frc/shuffleboard/SendableCameraWrapper.h"
 
-#include <cscore_oo.h>
+#include <functional>
+#include <memory>
+#include <string>
+
 #include <wpi/DenseMap.h>
 
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
 
-using namespace frc;
-
-namespace {
-constexpr const char* kProtocol = "camera_server://";
-wpi::DenseMap<int, std::unique_ptr<SendableCameraWrapper>> wrappers;
-}  // namespace
-
-SendableCameraWrapper& SendableCameraWrapper::Wrap(
-    const cs::VideoSource& source) {
-  return Wrap(source.GetHandle());
+namespace frc {
+namespace detail {
+std::shared_ptr<SendableCameraWrapper>& GetSendableCameraWrapper(
+    CS_Source source) {
+  static wpi::DenseMap<int, std::shared_ptr<SendableCameraWrapper>> wrappers;
+  return wrappers[static_cast<int>(source)];
 }
 
-SendableCameraWrapper& SendableCameraWrapper::Wrap(CS_Source source) {
-  auto& wrapper = wrappers[static_cast<int>(source)];
-  if (!wrapper)
-    wrapper = std::make_unique<SendableCameraWrapper>(source, private_init{});
-  return *wrapper;
-}
-
-SendableCameraWrapper::SendableCameraWrapper(CS_Source source,
-                                             const private_init&)
-    : m_uri(kProtocol) {
-  CS_Status status = 0;
-  auto name = cs::GetSourceName(source, &status);
-  SendableRegistry::GetInstance().Add(this, name);
-  m_uri += name;
+void AddToSendableRegistry(frc::Sendable* sendable, std::string name) {
+  SendableRegistry::GetInstance().Add(sendable, name);
 }
+}  // namespace detail
 
 void SendableCameraWrapper::InitSendable(SendableBuilder& builder) {
   builder.AddStringProperty(".ShuffleboardURI", [this] { return m_uri; },
                             nullptr);
 }
+}  // namespace frc

wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp

@@ -11,7 +11,6 @@
 #include <wpi/raw_ostream.h>
 
 #include "frc/shuffleboard/ComplexWidget.h"
-#include "frc/shuffleboard/SendableCameraWrapper.h"
 #include "frc/shuffleboard/ShuffleboardComponent.h"
 #include "frc/shuffleboard/ShuffleboardLayout.h"
 #include "frc/shuffleboard/SimpleWidget.h"
@@ -75,11 +74,6 @@ ComplexWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
   return *ptr;
 }
 
-ComplexWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
-                                          const cs::VideoSource& video) {
-  return Add(title, SendableCameraWrapper::Wrap(video));
-}
-
 ComplexWidget& ShuffleboardContainer::Add(Sendable& sendable) {
   auto name = SendableRegistry::GetInstance().GetName(&sendable);
   if (name.empty()) {
@@ -88,10 +82,6 @@ ComplexWidget& ShuffleboardContainer::Add(Sendable& sendable) {
   return Add(name, sendable);
 }
 
-ComplexWidget& ShuffleboardContainer::Add(const cs::VideoSource& video) {
-  return Add(SendableCameraWrapper::Wrap(video));
-}
-
 SimpleWidget& ShuffleboardContainer::Add(
     const wpi::Twine& title, std::shared_ptr<nt::Value> defaultValue) {
   CheckTitle(title);

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

@@ -23,6 +23,8 @@ std::shared_ptr<nt::NetworkTable> SendableBuilderImpl::GetTable() {
   return m_table;
 }
 
+bool SendableBuilderImpl::HasTable() const { return m_table != nullptr; }
+
 bool SendableBuilderImpl::IsActuator() const { return m_actuator; }
 
 void SendableBuilderImpl::UpdateTable() {
@@ -53,6 +55,8 @@ void SendableBuilderImpl::StopLiveWindowMode() {
   if (m_safeState) m_safeState();
 }
 
+void SendableBuilderImpl::ClearProperties() { m_properties.clear(); }
+
 void SendableBuilderImpl::SetSmartDashboardType(const wpi::Twine& type) {
   m_table->GetEntry(".type").SetString(type);
 }

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

@@ -16,3 +16,22 @@ std::atomic_int SendableChooserBase::s_instances{0};
 SendableChooserBase::SendableChooserBase() : m_instance{s_instances++} {
   SendableRegistry::GetInstance().Add(this, "SendableChooser", m_instance);
 }
+
+SendableChooserBase::SendableChooserBase(SendableChooserBase&& oth)
+    : SendableHelper(std::move(oth)),
+      m_defaultChoice(std::move(oth.m_defaultChoice)),
+      m_selected(std::move(oth.m_selected)),
+      m_haveSelected(std::move(oth.m_haveSelected)),
+      m_activeEntries(std::move(oth.m_activeEntries)),
+      m_instance(std::move(oth.m_instance)) {}
+
+SendableChooserBase& SendableChooserBase::operator=(SendableChooserBase&& oth) {
+  SendableHelper::operator=(oth);
+  std::scoped_lock lock(m_mutex, oth.m_mutex);
+  m_defaultChoice = std::move(oth.m_defaultChoice);
+  m_selected = std::move(oth.m_selected);
+  m_haveSelected = std::move(oth.m_haveSelected);
+  m_activeEntries = std::move(oth.m_activeEntries);
+  m_instance = std::move(oth.m_instance);
+  return *this;
+}

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

@@ -7,15 +7,22 @@
 
 #include "frc/smartdashboard/SendableRegistry.h"
 
+#include <memory>
+
 #include <wpi/DenseMap.h>
 #include <wpi/SmallVector.h>
+#include <wpi/UidVector.h>
 #include <wpi/mutex.h>
 
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableBuilderImpl.h"
+
 using namespace frc;
 
 struct SendableRegistry::Impl {
   struct Component {
     Sendable* sendable = nullptr;
+    SendableBuilderImpl builder;
     std::string name;
     std::string subsystem = "Ungrouped";
     Sendable* parent = nullptr;
@@ -37,17 +44,21 @@ struct SendableRegistry::Impl {
 
   wpi::mutex mutex;
 
-  wpi::DenseMap<void*, Component> components;
+  wpi::UidVector<std::unique_ptr<Component>, 32> components;
+  wpi::DenseMap<void*, UID> componentMap;
   int nextDataHandle = 0;
 
-  Component& GetOrAdd(Sendable* sendable);
+  Component& GetOrAdd(void* sendable, UID* uid = nullptr);
 };
 
 SendableRegistry::Impl::Component& SendableRegistry::Impl::GetOrAdd(
-    Sendable* sendable) {
-  auto& comp = components[sendable];
-  comp.sendable = sendable;
-  return comp;
+    void* sendable, UID* uid) {
+  UID& compUid = componentMap[sendable];
+  if (compUid == 0)
+    compUid = components.emplace_back(std::make_unique<Component>()) + 1;
+  if (uid) *uid = compUid;
+
+  return *components[compUid - 1];
 }
 
 SendableRegistry& SendableRegistry::GetInstance() {
@@ -58,6 +69,7 @@ SendableRegistry& SendableRegistry::GetInstance() {
 void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.name = name.str();
 }
 
@@ -65,6 +77,7 @@ void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& moduleType,
                            int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.SetName(moduleType, channel);
 }
 
@@ -72,6 +85,7 @@ void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& moduleType,
                            int moduleNumber, int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.SetName(moduleType, moduleNumber, channel);
 }
 
@@ -79,6 +93,7 @@ void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& subsystem,
                            const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.name = name.str();
   comp.subsystem = subsystem.str();
 }
@@ -86,6 +101,7 @@ void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& subsystem,
 void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.liveWindow = true;
   comp.name = name.str();
 }
@@ -94,6 +110,7 @@ void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& moduleType,
                              int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.liveWindow = true;
   comp.SetName(moduleType, channel);
 }
@@ -102,6 +119,7 @@ void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& moduleType,
                              int moduleNumber, int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.liveWindow = true;
   comp.SetName(moduleType, moduleNumber, channel);
 }
@@ -110,6 +128,7 @@ void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& subsystem,
                              const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.liveWindow = true;
   comp.name = name.str();
   comp.subsystem = subsystem.str();
@@ -117,82 +136,95 @@ void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& subsystem,
 
 void SendableRegistry::AddChild(Sendable* parent, void* child) {
   std::scoped_lock lock(m_impl->mutex);
-  auto& comp = m_impl->components[child];
+  auto& comp = m_impl->GetOrAdd(child);
   comp.parent = parent;
 }
 
 bool SendableRegistry::Remove(Sendable* sendable) {
   std::scoped_lock lock(m_impl->mutex);
-  return m_impl->components.erase(sendable);
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return false;
+  UID compUid = it->getSecond();
+  m_impl->components.erase(compUid - 1);
+  m_impl->componentMap.erase(it);
+  return true;
 }
 
 void SendableRegistry::Move(Sendable* to, Sendable* from) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(from);
-  if (it == m_impl->components.end()) return;
-  Impl::Component old = std::move(it->getSecond());
-  m_impl->components.erase(it);
-  m_impl->components[to] = std::move(old);
-  m_impl->components[to].sendable = to;
+  auto it = m_impl->componentMap.find(from);
+  if (it == m_impl->componentMap.end()) return;
+  UID compUid = it->getSecond();
+  m_impl->componentMap.erase(it);
+  m_impl->componentMap[to] = compUid;
+  auto& comp = *m_impl->components[compUid - 1];
+  comp.sendable = to;
+  if (comp.builder.HasTable()) {
+    // rebuild builder, as lambda captures can point to "from"
+    comp.builder.ClearProperties();
+    to->InitSendable(comp.builder);
+  }
 }
 
 bool SendableRegistry::Contains(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
-  return m_impl->components.count(sendable) != 0;
+  return m_impl->componentMap.count(sendable) != 0;
 }
 
 std::string SendableRegistry::GetName(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return std::string{};
-  return it->getSecond().name;
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return std::string{};
+  return m_impl->components[it->getSecond() - 1]->name;
 }
 
 void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().name = name.str();
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  m_impl->components[it->getSecond() - 1]->name = name.str();
 }
 
 void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
                                int channel) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().SetName(moduleType, channel);
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  m_impl->components[it->getSecond() - 1]->SetName(moduleType, channel);
 }
 
 void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
                                int moduleNumber, int channel) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().SetName(moduleType, moduleNumber, channel);
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  m_impl->components[it->getSecond() - 1]->SetName(moduleType, moduleNumber,
+                                                   channel);
 }
 
 void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& subsystem,
                                const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().name = name.str();
-  it->getSecond().subsystem = subsystem.str();
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  auto& comp = *m_impl->components[it->getSecond() - 1];
+  comp.name = name.str();
+  comp.subsystem = subsystem.str();
 }
 
 std::string SendableRegistry::GetSubsystem(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return std::string{};
-  return it->getSecond().subsystem;
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return std::string{};
+  return m_impl->components[it->getSecond() - 1]->subsystem;
 }
 
 void SendableRegistry::SetSubsystem(Sendable* sendable,
                                     const wpi::Twine& subsystem) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().subsystem = subsystem.str();
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  m_impl->components[it->getSecond() - 1]->subsystem = subsystem.str();
 }
 
 int SendableRegistry::GetDataHandle() {
@@ -204,9 +236,9 @@ std::shared_ptr<void> SendableRegistry::SetData(Sendable* sendable, int handle,
                                                 std::shared_ptr<void> data) {
   assert(handle >= 0);
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return nullptr;
-  auto& comp = it->getSecond();
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return nullptr;
+  auto& comp = *m_impl->components[it->getSecond() - 1];
   std::shared_ptr<void> rv;
   if (static_cast<size_t>(handle) < comp.data.size())
     rv = std::move(comp.data[handle]);
@@ -220,42 +252,72 @@ std::shared_ptr<void> SendableRegistry::GetData(Sendable* sendable,
                                                 int handle) {
   assert(handle >= 0);
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return nullptr;
-  auto& comp = it->getSecond();
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return nullptr;
+  auto& comp = *m_impl->components[it->getSecond() - 1];
   if (static_cast<size_t>(handle) >= comp.data.size()) return nullptr;
   return comp.data[handle];
 }
 
 void SendableRegistry::EnableLiveWindow(Sendable* sendable) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().liveWindow = true;
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  m_impl->components[it->getSecond() - 1]->liveWindow = true;
 }
 
 void SendableRegistry::DisableLiveWindow(Sendable* sendable) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().liveWindow = false;
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  m_impl->components[it->getSecond() - 1]->liveWindow = false;
+}
+
+SendableRegistry::UID SendableRegistry::GetUniqueId(Sendable* sendable) {
+  std::scoped_lock lock(m_impl->mutex);
+  UID uid;
+  auto& comp = m_impl->GetOrAdd(sendable, &uid);
+  comp.sendable = sendable;
+  return uid;
+}
+
+Sendable* SendableRegistry::GetSendable(UID uid) {
+  if (uid == 0) return nullptr;
+  std::scoped_lock lock(m_impl->mutex);
+  return m_impl->components[uid - 1]->sendable;
+}
+
+void SendableRegistry::Publish(UID sendableUid,
+                               std::shared_ptr<NetworkTable> table) {
+  std::scoped_lock lock(m_impl->mutex);
+  if (sendableUid == 0) return;
+  auto& comp = *m_impl->components[sendableUid - 1];
+  comp.builder = SendableBuilderImpl{};  // clear any current builder
+  comp.builder.SetTable(table);
+  comp.sendable->InitSendable(comp.builder);
+  comp.builder.UpdateTable();
+  comp.builder.StartListeners();
+}
+
+void SendableRegistry::Update(UID sendableUid) {
+  if (sendableUid == 0) return;
+  std::scoped_lock lock(m_impl->mutex);
+  m_impl->components[sendableUid - 1]->builder.UpdateTable();
 }
 
 void SendableRegistry::ForeachLiveWindow(
     int dataHandle,
-    wpi::function_ref<void(Sendable* sendable, wpi::StringRef name,
-                           wpi::StringRef subsystem, Sendable* parent,
-                           std::shared_ptr<void>& data)>
-        callback) const {
+    wpi::function_ref<void(CallbackData& data)> callback) const {
   assert(dataHandle >= 0);
   std::scoped_lock lock(m_impl->mutex);
-  for (auto& i : m_impl->components) {
-    auto& comp = i.getSecond();
-    if (comp.sendable && comp.liveWindow) {
-      if (static_cast<size_t>(dataHandle) >= comp.data.size())
-        comp.data.resize(dataHandle + 1);
-      callback(comp.sendable, comp.name, comp.subsystem, comp.parent,
-               comp.data[dataHandle]);
+  for (auto&& comp : m_impl->components) {
+    if (comp->sendable && comp->liveWindow) {
+      if (static_cast<size_t>(dataHandle) >= comp->data.size())
+        comp->data.resize(dataHandle + 1);
+      CallbackData cbdata{comp->sendable,         comp->name,
+                          comp->subsystem,        comp->parent,
+                          comp->data[dataHandle], comp->builder};
+      callback(cbdata);
     }
   }
 }

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

@@ -14,27 +14,17 @@
 #include <wpi/mutex.h>
 
 #include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilderImpl.h"
 #include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 namespace {
-class SmartDashboardData {
- public:
-  SmartDashboardData() = default;
-  explicit SmartDashboardData(Sendable* sendable_) : sendable(sendable_) {}
-
-  Sendable* sendable = nullptr;
-  SendableBuilderImpl builder;
-};
-
 class Singleton {
  public:
   static Singleton& GetInstance();
 
   std::shared_ptr<nt::NetworkTable> table;
-  wpi::StringMap<SmartDashboardData> tablesToData;
+  wpi::StringMap<SendableRegistry::UID> tablesToData;
   wpi::mutex tablesToDataMutex;
 
  private:
@@ -98,14 +88,13 @@ void SmartDashboard::PutData(wpi::StringRef key, Sendable* data) {
   }
   auto& inst = Singleton::GetInstance();
   std::scoped_lock lock(inst.tablesToDataMutex);
-  auto& sddata = inst.tablesToData[key];
-  if (!sddata.sendable || sddata.sendable != data) {
-    sddata = SmartDashboardData(data);
+  auto& uid = inst.tablesToData[key];
+  auto& registry = SendableRegistry::GetInstance();
+  Sendable* sddata = registry.GetSendable(uid);
+  if (sddata != data) {
+    uid = registry.GetUniqueId(data);
     auto dataTable = inst.table->GetSubTable(key);
-    sddata.builder.SetTable(dataTable);
-    data->InitSendable(sddata.builder);
-    sddata.builder.UpdateTable();
-    sddata.builder.StartListeners();
+    registry.Publish(uid, dataTable);
     dataTable->GetEntry(".name").SetString(key);
   }
 }
@@ -122,12 +111,12 @@ void SmartDashboard::PutData(Sendable* value) {
 Sendable* SmartDashboard::GetData(wpi::StringRef key) {
   auto& inst = Singleton::GetInstance();
   std::scoped_lock lock(inst.tablesToDataMutex);
-  auto data = inst.tablesToData.find(key);
-  if (data == inst.tablesToData.end()) {
+  auto it = inst.tablesToData.find(key);
+  if (it == inst.tablesToData.end()) {
     wpi_setGlobalWPIErrorWithContext(SmartDashboardMissingKey, key);
     return nullptr;
   }
-  return data->getValue().sendable;
+  return SendableRegistry::GetInstance().GetSendable(it->getValue());
 }
 
 bool SmartDashboard::PutBoolean(wpi::StringRef keyName, bool value) {
@@ -262,10 +251,9 @@ void SmartDashboard::PostListenerTask(std::function<void()> task) {
 }
 
 void SmartDashboard::UpdateValues() {
+  auto& registry = SendableRegistry::GetInstance();
   auto& inst = Singleton::GetInstance();
   std::scoped_lock lock(inst.tablesToDataMutex);
-  for (auto& i : inst.tablesToData) {
-    i.getValue().builder.UpdateTable();
-  }
+  for (auto& i : inst.tablesToData) registry.Update(i.getValue());
   listenerExecutor.RunListenerTasks();
 }

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

@@ -11,39 +11,22 @@
 
 using namespace frc;
 
-std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromWaypoints(
-    const Pose2d& start, std::vector<Translation2d> waypoints,
-    const Pose2d& end) {
+std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
+    const Spline<3>::ControlVector& start, std::vector<Translation2d> waypoints,
+    const Spline<3>::ControlVector& end) {
   std::vector<CubicHermiteSpline> splines;
 
-  double scalar;
-  // This just makes the splines look better.
-  if (waypoints.empty()) {
-    scalar = 1.2 * start.Translation().Distance(end.Translation()).to<double>();
-  } else {
-    scalar = 1.2 * start.Translation().Distance(waypoints.front()).to<double>();
-  }
-
-  std::array<double, 2> startXControlVector{
-      start.Translation().X().to<double>(), start.Rotation().Cos() * scalar};
-
-  std::array<double, 2> startYControlVector{
-      start.Translation().Y().to<double>(), start.Rotation().Sin() * scalar};
-
-  // This just makes the splines look better.
-  if (!waypoints.empty()) {
-    scalar = 1.2 * end.Translation().Distance(waypoints.back()).to<double>();
-  }
-
-  std::array<double, 2> endXControlVector{end.Translation().X().to<double>(),
-                                          end.Rotation().Cos() * scalar};
-
-  std::array<double, 2> endYControlVector{end.Translation().Y().to<double>(),
-                                          end.Rotation().Sin() * scalar};
+  std::array<double, 2> xInitial = start.x;
+  std::array<double, 2> yInitial = start.y;
+  std::array<double, 2> xFinal = end.x;
+  std::array<double, 2> yFinal = end.y;
 
   if (waypoints.size() > 1) {
-    waypoints.emplace(waypoints.begin(), start.Translation());
-    waypoints.emplace_back(end.Translation());
+    waypoints.emplace(waypoints.begin(),
+                      Translation2d{units::meter_t(xInitial[0]),
+                                    units::meter_t(yInitial[0])});
+    waypoints.emplace_back(
+        Translation2d{units::meter_t(xFinal[0]), units::meter_t(yFinal[0])});
 
     std::vector<double> a;
     std::vector<double> b(waypoints.size() - 2, 4.0);
@@ -53,7 +36,7 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromWaypoints(
         fy(waypoints.size() - 2, 0.0);
 
     a.emplace_back(0);
-    for (unsigned int i = 0; i < waypoints.size() - 3; i++) {
+    for (size_t i = 0; i < waypoints.size() - 3; ++i) {
       a.emplace_back(1);
       c.emplace_back(1);
     }
@@ -61,12 +44,12 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromWaypoints(
 
     dx.emplace_back(
         3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
-        startXControlVector[1]);
+        xInitial[1]);
     dy.emplace_back(
         3 * (waypoints[2].Y().to<double>() - waypoints[0].Y().to<double>()) -
-        startYControlVector[1]);
+        yInitial[1]);
     if (waypoints.size() > 4) {
-      for (unsigned int i = 1; i <= waypoints.size() - 4; i++) {
+      for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
         dx.emplace_back(3 * (waypoints[i + 1].X().to<double>() -
                              waypoints[i - 1].X().to<double>()));
         dy.emplace_back(3 * (waypoints[i + 1].Y().to<double>() -
@@ -75,20 +58,20 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromWaypoints(
     }
     dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to<double>() -
                          waypoints[waypoints.size() - 3].X().to<double>()) -
-                    endXControlVector[1]);
+                    xFinal[1]);
     dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to<double>() -
                          waypoints[waypoints.size() - 3].Y().to<double>()) -
-                    endYControlVector[1]);
+                    yFinal[1]);
 
     ThomasAlgorithm(a, b, c, dx, &fx);
     ThomasAlgorithm(a, b, c, dy, &fy);
 
-    fx.emplace(fx.begin(), startXControlVector[1]);
-    fx.emplace_back(endXControlVector[1]);
-    fy.emplace(fy.begin(), startYControlVector[1]);
-    fy.emplace_back(endYControlVector[1]);
+    fx.emplace(fx.begin(), xInitial[1]);
+    fx.emplace_back(xFinal[1]);
+    fy.emplace(fy.begin(), yInitial[1]);
+    fy.emplace_back(yFinal[1]);
 
-    for (unsigned int i = 0; i < fx.size() - 1; i++) {
+    for (size_t i = 0; i < fx.size() - 1; ++i) {
       // Create the spline.
       const CubicHermiteSpline spline{
           {waypoints[i].X().to<double>(), fx[i]},
@@ -99,39 +82,69 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromWaypoints(
       splines.push_back(spline);
     }
   } else if (waypoints.size() == 1) {
-    const double xDeriv = (3 * (end.Translation().X().to<double>() -
-                                start.Translation().X().to<double>()) -
-                           endXControlVector[1] - startXControlVector[1]) /
-                          4.0;
-    const double yDeriv = (3 * (end.Translation().Y().to<double>() -
-                                start.Translation().Y().to<double>()) -
-                           endYControlVector[1] - startYControlVector[1]) /
-                          4.0;
+    const double xDeriv =
+        (3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
+    const double yDeriv =
+        (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
 
     std::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
                                             xDeriv};
     std::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
                                             yDeriv};
 
-    splines.emplace_back(startXControlVector, midXControlVector,
-                         startYControlVector, midYControlVector);
-    splines.emplace_back(midXControlVector, endXControlVector,
-                         midYControlVector, endYControlVector);
+    splines.emplace_back(xInitial, midXControlVector, yInitial,
+                         midYControlVector);
+    splines.emplace_back(midXControlVector, xFinal, midYControlVector, yFinal);
 
   } else {
     // Create the spline.
-    const CubicHermiteSpline spline{startXControlVector, endXControlVector,
-                                    startYControlVector, endYControlVector};
+    const CubicHermiteSpline spline{xInitial, xFinal, yInitial, yFinal};
     splines.push_back(spline);
   }
 
   return splines;
 }
 
-std::vector<QuinticHermiteSpline> SplineHelper::QuinticSplinesFromWaypoints(
-    const std::vector<Pose2d>& waypoints) {
+std::vector<QuinticHermiteSpline>
+SplineHelper::QuinticSplinesFromControlVectors(
+    const std::vector<Spline<5>::ControlVector>& controlVectors) {
   std::vector<QuinticHermiteSpline> splines;
-  for (unsigned int i = 0; i < waypoints.size() - 1; i++) {
+  for (size_t i = 0; i < controlVectors.size() - 1; ++i) {
+    auto& xInitial = controlVectors[i].x;
+    auto& yInitial = controlVectors[i].y;
+    auto& xFinal = controlVectors[i + 1].x;
+    auto& yFinal = controlVectors[i + 1].y;
+    splines.emplace_back(xInitial, xFinal, yInitial, yFinal);
+  }
+  return splines;
+}
+
+std::array<Spline<3>::ControlVector, 2>
+SplineHelper::CubicControlVectorsFromWaypoints(
+    const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+    const Pose2d& end) {
+  double scalar;
+  if (interiorWaypoints.empty()) {
+    scalar = 1.2 * start.Translation().Distance(end.Translation()).to<double>();
+  } else {
+    scalar =
+        1.2 *
+        start.Translation().Distance(interiorWaypoints.front()).to<double>();
+  }
+  const auto initialCV = CubicControlVector(scalar, start);
+  if (!interiorWaypoints.empty()) {
+    scalar =
+        1.2 * end.Translation().Distance(interiorWaypoints.back()).to<double>();
+  }
+  const auto finalCV = CubicControlVector(scalar, end);
+  return {initialCV, finalCV};
+}
+
+std::vector<Spline<5>::ControlVector>
+SplineHelper::QuinticControlVectorsFromWaypoints(
+    const std::vector<Pose2d>& waypoints) {
+  std::vector<Spline<5>::ControlVector> vectors;
+  for (size_t i = 0; i < waypoints.size() - 1; ++i) {
     auto& p0 = waypoints[i];
     auto& p1 = waypoints[i + 1];
 
@@ -139,19 +152,10 @@ std::vector<QuinticHermiteSpline> SplineHelper::QuinticSplinesFromWaypoints(
     const auto scalar =
         1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
 
-    const std::array<double, 3> xInitialControlVector{
-        p0.Translation().X().to<double>(), p0.Rotation().Cos() * scalar, 0.0};
-    const std::array<double, 3> xFinalControlVector{
-        p1.Translation().X().to<double>(), p1.Rotation().Cos() * scalar, 0.0};
-    const std::array<double, 3> yInitialControlVector{
-        p0.Translation().Y().to<double>(), p0.Rotation().Sin() * scalar, 0.0};
-    const std::array<double, 3> yFinalControlVector{
-        p1.Translation().Y().to<double>(), p1.Rotation().Sin() * scalar, 0.0};
-
-    splines.emplace_back(xInitialControlVector, xFinalControlVector,
-                         yInitialControlVector, yFinalControlVector);
+    vectors.push_back(QuinticControlVector(scalar, p0));
+    vectors.push_back(QuinticControlVector(scalar, p1));
   }
-  return splines;
+  return vectors;
 }
 
 void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,
@@ -176,7 +180,7 @@ void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,
   d_star[0] = d[0] / b[0];
 
   // Create the c_star and d_star coefficients in the forward sweep
-  for (unsigned int i = 1; i < N; i++) {
+  for (size_t i = 1; i < N; ++i) {
     double m = 1.0 / (b[i] - a[i] * c_star[i - 1]);
     c_star[i] = c[i] * m;
     d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m;

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

@@ -15,63 +15,78 @@
 using namespace frc;
 
 Trajectory TrajectoryGenerator::GenerateTrajectory(
-    const std::vector<Pose2d>& waypoints,
-    std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
-    units::meters_per_second_t startVelocity,
-    units::meters_per_second_t endVelocity,
-    units::meters_per_second_t maxVelocity,
-    units::meters_per_second_squared_t maxAcceleration, bool reversed) {
+    Spline<3>::ControlVector initial,
+    const std::vector<Translation2d>& interiorWaypoints,
+    Spline<3>::ControlVector end, const TrajectoryConfig& config) {
   const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
 
   // Make theta normal for trajectory generation if path is reversed.
-  std::vector<Pose2d> newWaypoints;
-  newWaypoints.reserve(waypoints.size());
-  for (auto&& point : waypoints) {
-    newWaypoints.push_back(reversed ? point + flip : point);
+  // Flip the headings.
+  if (config.IsReversed()) {
+    initial.x[1] *= -1;
+    initial.y[1] *= -1;
+    end.x[1] *= -1;
+    end.y[1] *= -1;
   }
 
-  auto points = SplinePointsFromSplines(
-      SplineHelper::QuinticSplinesFromWaypoints(newWaypoints));
+  auto points =
+      SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
+          initial, interiorWaypoints, end));
 
   // After trajectory generation, flip theta back so it's relative to the
   // field. Also fix curvature.
-  if (reversed) {
+  if (config.IsReversed()) {
     for (auto& point : points) {
       point = {point.first + flip, -point.second};
     }
   }
 
   return TrajectoryParameterizer::TimeParameterizeTrajectory(
-      points, std::move(constraints), startVelocity, endVelocity, maxVelocity,
-      maxAcceleration, reversed);
+      points, config.Constraints(), config.StartVelocity(),
+      config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
+      config.IsReversed());
 }
 
 Trajectory TrajectoryGenerator::GenerateTrajectory(
-    const Pose2d& start, const std::vector<Translation2d>& waypoints,
-    const Pose2d& end,
-    std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
-    units::meters_per_second_t startVelocity,
-    units::meters_per_second_t endVelocity,
-    units::meters_per_second_t maxVelocity,
-    units::meters_per_second_squared_t maxAcceleration, bool reversed) {
+    const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+    const Pose2d& end, const TrajectoryConfig& config) {
+  auto [startCV, endCV] = SplineHelper::CubicControlVectorsFromWaypoints(
+      start, interiorWaypoints, end);
+  return GenerateTrajectory(startCV, interiorWaypoints, endCV, config);
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+    std::vector<Spline<5>::ControlVector> controlVectors,
+    const TrajectoryConfig& config) {
   const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
   // Make theta normal for trajectory generation if path is reversed.
-
-  const Pose2d newStart = reversed ? start + flip : start;
-  const Pose2d newEnd = reversed ? end + flip : end;
+  if (config.IsReversed()) {
+    for (auto& vector : controlVectors) {
+      // Flip the headings.
+      vector.x[1] *= -1;
+      vector.y[1] *= -1;
+    }
+  }
 
   auto points = SplinePointsFromSplines(
-      SplineHelper::CubicSplinesFromWaypoints(newStart, waypoints, newEnd));
+      SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
 
   // After trajectory generation, flip theta back so it's relative to the
   // field. Also fix curvature.
-  if (reversed) {
+  if (config.IsReversed()) {
     for (auto& point : points) {
       point = {point.first + flip, -point.second};
     }
   }
 
   return TrajectoryParameterizer::TimeParameterizeTrajectory(
-      points, std::move(constraints), startVelocity, endVelocity, maxVelocity,
-      maxAcceleration, reversed);
+      points, config.Constraints(), config.StartVelocity(),
+      config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
+      config.IsReversed());
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+    const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
+  return GenerateTrajectory(
+      SplineHelper::QuinticControlVectorsFromWaypoints(waypoints), config);
 }

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

@@ -35,7 +35,7 @@ using namespace frc;
 
 Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
     const std::vector<PoseWithCurvature>& points,
-    std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
+    const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
     units::meters_per_second_t startVelocity,
     units::meters_per_second_t endVelocity,
     units::meters_per_second_t maxVelocity,

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

@@ -7,10 +7,13 @@
 
 #include "frc/RobotBase.h"
 
+#ifdef __FRC_ROBORIO__
+#include <dlfcn.h>
+#endif
+
 #include <cstdio>
 
 #include <cameraserver/CameraServerShared.h>
-#include <cscore.h>
 #include <hal/HAL.h>
 #include <networktables/NetworkTableInstance.h>
 
@@ -22,6 +25,8 @@
 #include "frc/livewindow/LiveWindow.h"
 #include "frc/smartdashboard/SmartDashboard.h"
 
+typedef void (*SetCameraServerSharedFP)(frc::CameraServerShared* shared);
+
 using namespace frc;
 
 int frc::RunHALInitialization() {
@@ -65,7 +70,36 @@ class WPILibCameraServerShared : public frc::CameraServerShared {
 }  // namespace
 
 static void SetupCameraServerShared() {
-  SetCameraServerShared(std::make_unique<WPILibCameraServerShared>());
+#ifdef __FRC_ROBORIO__
+#ifdef DYNAMIC_CAMERA_SERVER
+#ifdef DYNAMIC_CAMERA_SERVER_DEBUG
+  auto cameraServerLib = dlopen("libcameraserverd.so", RTLD_NOW);
+#else
+  auto cameraServerLib = dlopen("libcameraserver.so", RTLD_NOW);
+#endif
+
+  if (!cameraServerLib) {
+    wpi::outs() << "Camera Server Library Not Found\n";
+    wpi::outs().flush();
+    return;
+  }
+  auto symbol = dlsym(cameraServerLib, "CameraServer_SetCameraServerShared");
+  if (symbol) {
+    auto setCameraServerShared = (SetCameraServerSharedFP)symbol;
+    setCameraServerShared(new WPILibCameraServerShared{});
+    wpi::outs() << "Set Camera Server Shared\n";
+    wpi::outs().flush();
+  } else {
+    wpi::outs() << "Camera Server Shared Symbol Missing\n";
+    wpi::outs().flush();
+  }
+#else
+  CameraServer_SetCameraServerShared(new WPILibCameraServerShared{});
+#endif
+#else
+  wpi::outs() << "Not loading CameraServerShared\n";
+  wpi::outs().flush();
+#endif
 }
 
 bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }
@@ -121,6 +155,6 @@ RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
 RobotBase::RobotBase(RobotBase&&) noexcept
     : m_ds(DriverStation::GetInstance()) {}
 
-RobotBase::~RobotBase() { cs::Shutdown(); }
+RobotBase::~RobotBase() {}
 
 RobotBase& RobotBase::operator=(RobotBase&&) noexcept { return *this; }

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -179,7 +179,7 @@ class GenericHID : public ErrorBase {
   void SetRumble(RumbleType type, double value);
 
  private:
-  DriverStation& m_ds;
+  DriverStation* m_ds;
   int m_port;
   int m_outputs = 0;
   uint16_t m_leftRumble = 0;

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

@@ -7,8 +7,6 @@
 
 #pragma once
 
-#include <atomic>
-
 #include "frc/DigitalOutput.h"
 #include "frc/ErrorBase.h"
 #include "frc/MotorSafety.h"
@@ -102,7 +100,7 @@ class NidecBrushless : public SpeedController,
 
  private:
   bool m_isInverted = false;
-  std::atomic_bool m_disabled{false};
+  bool m_disabled = false;
   DigitalOutput m_dio;
   PWM m_pwm;
   double m_speed = 0.0;

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

@@ -50,6 +50,7 @@ class PIDBase : public PIDInterface,
    * @param source The PIDSource object that is used to get values
    * @param output The PIDOutput object that is set to the output value
    */
+  WPI_DEPRECATED("All APIs which use this have been deprecated.")
   PIDBase(double p, double i, double d, PIDSource& source, PIDOutput& output);
 
   /**
@@ -61,14 +62,12 @@ class PIDBase : public PIDInterface,
    * @param source The PIDSource object that is used to get values
    * @param output The PIDOutput object that is set to the output value
    */
+  WPI_DEPRECATED("All APIs which use this have been deprecated.")
   PIDBase(double p, double i, double d, double f, PIDSource& source,
           PIDOutput& output);
 
   virtual ~PIDBase() = default;
 
-  PIDBase(PIDBase&&) = default;
-  PIDBase& operator=(PIDBase&&) = default;
-
   /**
    * Return the current PID result.
    *

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

@@ -102,9 +102,6 @@ class PIDController : public PIDBase, public Controller {
 
   ~PIDController() override;
 
-  PIDController(PIDController&&) = default;
-  PIDController& operator=(PIDController&&) = default;
-
   /**
    * Begin running the PIDController.
    */

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,10 +7,13 @@
 
 #pragma once
 
+#include <wpi/deprecated.h>
+
 namespace frc {
 
 class PIDInterface {
  public:
+  WPI_DEPRECATED("All APIs which use this have been deprecated.")
   PIDInterface() = default;
   PIDInterface(PIDInterface&&) = default;
   PIDInterface& operator=(PIDInterface&&) = default;

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -33,9 +33,6 @@ class Resource : public ErrorBase {
  public:
   virtual ~Resource() = default;
 
-  Resource(Resource&&) = default;
-  Resource& operator=(Resource&&) = default;
-
   /**
    * Factory method to create a Resource allocation-tracker *if* needed.
    *

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

@@ -12,11 +12,10 @@
 #include <wpi/mutex.h>
 
 #include "frc/Base.h"
+#include "frc2/Timer.h"
 
 namespace frc {
 
-using TimerInterruptHandler = void (*)(void* param);
-
 /**
  * Pause the task for a specified time.
  *
@@ -57,8 +56,10 @@ class Timer {
 
   virtual ~Timer() = default;
 
-  Timer(Timer&& rhs);
-  Timer& operator=(Timer&& rhs);
+  Timer(const Timer& rhs) = default;
+  Timer& operator=(const Timer& rhs) = default;
+  Timer(Timer&& rhs) = default;
+  Timer& operator=(Timer&& rhs) = default;
 
   /**
    * Get the current time from the timer. If the clock is running it is derived
@@ -102,19 +103,8 @@ class Timer {
    * @param period The period to check for (in seconds).
    * @return       True if the period has passed.
    */
-  WPI_DEPRECATED("Use unit-safe HasPeriodPassed method instead.")
   bool HasPeriodPassed(double period);
 
-  /**
-   * Check if the period specified has passed and if it has, advance the start
-   * time by that period. This is useful to decide if it's time to do periodic
-   * work without drifting later by the time it took to get around to checking.
-   *
-   * @param period The period to check for.
-   * @return       True if the period has passed.
-   */
-  bool HasPeriodPassed(units::second_t period);
-
   /**
    * Return the FPGA system clock time in seconds.
    *
@@ -146,10 +136,7 @@ class Timer {
   static const double kRolloverTime;
 
  private:
-  double m_startTime = 0.0;
-  double m_accumulatedTime = 0.0;
-  bool m_running = false;
-  mutable wpi::mutex m_mutex;
+  frc2::Timer m_timer;
 };
 
 }  // namespace frc

wpilibc/src/main/native/include/frc/buttons/Trigger.h

@@ -34,8 +34,10 @@ class Trigger : public Sendable, public SendableHelper<Trigger> {
   Trigger() = default;
   ~Trigger() override = default;
 
-  Trigger(Trigger&&) = default;
-  Trigger& operator=(Trigger&&) = default;
+  Trigger(const Trigger& rhs);
+  Trigger& operator=(const Trigger& rhs);
+  Trigger(Trigger&& rhs);
+  Trigger& operator=(Trigger&& rhs);
 
   bool Grab();
   virtual bool Get() = 0;

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

@@ -108,6 +108,13 @@ class ProfiledPIDController : public Sendable,
    */
   units::second_t GetPeriod() const;
 
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired unprofiled setpoint.
+   */
+  void SetGoal(TrapezoidProfile::State goal);
+
   /**
    * Sets the goal for the ProfiledPIDController.
    *
@@ -118,7 +125,7 @@ class ProfiledPIDController : public Sendable,
   /**
    * Gets the goal for the ProfiledPIDController.
    */
-  units::meter_t GetGoal() const;
+  TrapezoidProfile::State GetGoal() const;
 
   /**
    * Returns true if the error is within the tolerance of the error.
@@ -139,7 +146,7 @@ class ProfiledPIDController : public Sendable,
    *
    * @return The current setpoint.
    */
-  double GetSetpoint() const;
+  TrapezoidProfile::State GetSetpoint() const;
 
   /**
    * Returns true if the error is within the tolerance of the error.
@@ -208,7 +215,7 @@ class ProfiledPIDController : public Sendable,
    *
    * @param measurement The current measurement of the process variable.
    */
-  double Calculate(double measurement);
+  double Calculate(units::meter_t measurement);
 
   /**
    * Returns the next output of the PID controller.
@@ -216,7 +223,15 @@ class ProfiledPIDController : public Sendable,
    * @param measurement The current measurement of the process variable.
    * @param goal The new goal of the controller.
    */
-  double Calculate(double measurement, units::meter_t goal);
+  double Calculate(units::meter_t measurement, TrapezoidProfile::State goal);
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   */
+  double Calculate(units::meter_t measurement, units::meter_t goal);
 
   /**
    * Returns the next output of the PID controller.
@@ -225,7 +240,7 @@ class ProfiledPIDController : public Sendable,
    * @param goal        The new goal of the controller.
    * @param constraints Velocity and acceleration constraints for goal.
    */
-  double Calculate(double measurement, units::meter_t goal,
+  double Calculate(units::meter_t measurement, units::meter_t goal,
                    frc::TrapezoidProfile::Constraints constraints);
 
   /**

wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h

@@ -212,8 +212,8 @@ class DifferentialDrive : public RobotDriveBase,
   void InitSendable(SendableBuilder& builder) override;
 
  private:
-  SpeedController& m_leftMotor;
-  SpeedController& m_rightMotor;
+  SpeedController* m_leftMotor;
+  SpeedController* m_rightMotor;
 
   double m_quickStopThreshold = kDefaultQuickStopThreshold;
   double m_quickStopAlpha = kDefaultQuickStopAlpha;

wpilibc/src/main/native/include/frc/drive/KilloughDrive.h

@@ -132,9 +132,9 @@ class KilloughDrive : public RobotDriveBase,
   void InitSendable(SendableBuilder& builder) override;
 
  private:
-  SpeedController& m_leftMotor;
-  SpeedController& m_rightMotor;
-  SpeedController& m_backMotor;
+  SpeedController* m_leftMotor;
+  SpeedController* m_rightMotor;
+  SpeedController* m_backMotor;
 
   Vector2d m_leftVec;
   Vector2d m_rightVec;

wpilibc/src/main/native/include/frc/drive/MecanumDrive.h

@@ -137,10 +137,10 @@ class MecanumDrive : public RobotDriveBase,
   void InitSendable(SendableBuilder& builder) override;
 
  private:
-  SpeedController& m_frontLeftMotor;
-  SpeedController& m_rearLeftMotor;
-  SpeedController& m_frontRightMotor;
-  SpeedController& m_rearRightMotor;
+  SpeedController* m_frontLeftMotor;
+  SpeedController* m_rearLeftMotor;
+  SpeedController* m_frontRightMotor;
+  SpeedController* m_rearRightMotor;
 
   double m_rightSideInvertMultiplier = -1.0;
 

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

@@ -11,6 +11,7 @@
 
 #include "DifferentialDriveKinematics.h"
 #include "frc/geometry/Pose2d.h"
+#include "frc2/Timer.h"
 
 namespace frc {
 /**
@@ -47,6 +48,12 @@ class DifferentialDriveOdometry {
     m_previousAngle = pose.Rotation();
   }
 
+  /**
+   * Returns the position of the robot on the field.
+   * @return The pose of the robot.
+   */
+  const Pose2d& GetPose() const { return m_pose; }
+
   /**
    * Updates the robot's position on the field using forward kinematics and
    * integration of the pose over time. This method takes in the current time as
@@ -80,9 +87,7 @@ class DifferentialDriveOdometry {
    */
   const Pose2d& Update(const Rotation2d& angle,
                        const DifferentialDriveWheelSpeeds& wheelSpeeds) {
-    const auto now = std::chrono::system_clock::now().time_since_epoch();
-    units::second_t time{now};
-    return UpdateWithTime(time, angle, wheelSpeeds);
+    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
   }
 
  private:

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

@@ -12,6 +12,7 @@
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+#include "frc2/Timer.h"
 
 namespace frc {
 
@@ -45,6 +46,12 @@ class MecanumDriveOdometry {
     m_previousAngle = pose.Rotation();
   }
 
+  /**
+   * Returns the position of the robot on the field.
+   * @return The pose of the robot.
+   */
+  const Pose2d& GetPose() const { return m_pose; }
+
   /**
    * Updates the robot's position on the field using forward kinematics and
    * integration of the pose over time. This method takes in the current time as
@@ -78,9 +85,7 @@ class MecanumDriveOdometry {
    */
   const Pose2d& Update(const Rotation2d& angle,
                        MecanumDriveWheelSpeeds wheelSpeeds) {
-    const auto now = std::chrono::system_clock::now().time_since_epoch();
-    units::second_t time{now};
-    return UpdateWithTime(time, angle, wheelSpeeds);
+    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
   }
 
  private:

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

@@ -16,6 +16,7 @@
 #include "SwerveDriveKinematics.h"
 #include "SwerveModuleState.h"
 #include "frc/geometry/Pose2d.h"
+#include "frc2/Timer.h"
 
 namespace frc {
 
@@ -50,6 +51,12 @@ class SwerveDriveOdometry {
     m_previousAngle = pose.Rotation();
   }
 
+  /**
+   * Returns the position of the robot on the field.
+   * @return The pose of the robot.
+   */
+  const Pose2d& GetPose() const { return m_pose; }
+
   /**
    * Updates the robot's position on the field using forward kinematics and
    * integration of the pose over time. This method takes in the current time as
@@ -89,9 +96,8 @@ class SwerveDriveOdometry {
   template <typename... ModuleStates>
   const Pose2d& Update(const Rotation2d& angle,
                        ModuleStates&&... moduleStates) {
-    const auto now = std::chrono::system_clock::now().time_since_epoch();
-    units::second_t time{now};
-    return UpdateWithTime(time, angle, moduleStates...);
+    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle,
+                          moduleStates...);
   }
 
  private:

wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h

@@ -7,19 +7,34 @@
 
 #pragma once
 
+#include <functional>
+#include <memory>
 #include <string>
 
-#include <cscore_c.h>
+#ifndef DYNAMIC_CAMERA_SERVER
+#include <cscore_oo.h>
+#else
+namespace cs {
+class VideoSource;
+}  // namespace cs
+typedef int CS_Handle;
+typedef CS_Handle CS_Source;
+#endif
 
 #include "frc/smartdashboard/Sendable.h"
 #include "frc/smartdashboard/SendableHelper.h"
 
-namespace cs {
-class VideoSource;
-}  // namespace cs
-
 namespace frc {
 
+class SendableCameraWrapper;
+
+namespace detail {
+constexpr const char* kProtocol = "camera_server://";
+std::shared_ptr<SendableCameraWrapper>& GetSendableCameraWrapper(
+    CS_Source source);
+void AddToSendableRegistry(Sendable* sendable, std::string name);
+}  // namespace detail
+
 /**
  * A wrapper to make video sources sendable and usable from Shuffleboard.
  */
@@ -54,4 +69,27 @@ class SendableCameraWrapper : public Sendable,
   std::string m_uri;
 };
 
+#ifndef DYNAMIC_CAMERA_SERVER
+inline SendableCameraWrapper::SendableCameraWrapper(CS_Source source,
+                                                    const private_init&)
+    : m_uri(detail::kProtocol) {
+  CS_Status status = 0;
+  auto name = cs::GetSourceName(source, &status);
+  detail::AddToSendableRegistry(this, name);
+  m_uri += name;
+}
+
+inline SendableCameraWrapper& SendableCameraWrapper::Wrap(
+    const cs::VideoSource& source) {
+  return Wrap(source.GetHandle());
+}
+
+inline SendableCameraWrapper& SendableCameraWrapper::Wrap(CS_Source source) {
+  auto& wrapper = detail::GetSendableCameraWrapper(source);
+  if (!wrapper)
+    wrapper = std::make_shared<SendableCameraWrapper>(source, private_init{});
+  return *wrapper;
+}
+#endif
+
 }  // namespace frc

wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h

@@ -501,3 +501,17 @@ class ShuffleboardContainer : public virtual ShuffleboardValue,
 #include "frc/shuffleboard/ComplexWidget.h"
 #include "frc/shuffleboard/ShuffleboardLayout.h"
 #include "frc/shuffleboard/SimpleWidget.h"
+
+#ifndef DYNAMIC_CAMERA_SERVER
+#include "frc/shuffleboard/SendableCameraWrapper.h"
+
+inline frc::ComplexWidget& frc::ShuffleboardContainer::Add(
+    const cs::VideoSource& video) {
+  return Add(frc::SendableCameraWrapper::Wrap(video));
+}
+
+inline frc::ComplexWidget& frc::ShuffleboardContainer::Add(
+    const wpi::Twine& title, const cs::VideoSource& video) {
+  return Add(title, frc::SendableCameraWrapper::Wrap(video));
+}
+#endif

wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -45,6 +45,12 @@ class SendableBuilderImpl : public SendableBuilder {
    */
   std::shared_ptr<nt::NetworkTable> GetTable();
 
+  /**
+   * Return whether this sendable has an associated table.
+   * @return True if it has a table, false if not.
+   */
+  bool HasTable() const;
+
   /**
    * Return whether this sendable should be treated as an actuator.
    * @return True if actuator, false if not.
@@ -78,6 +84,11 @@ class SendableBuilderImpl : public SendableBuilder {
    */
   void StopLiveWindowMode();
 
+  /**
+   * Clear properties.
+   */
+  void ClearProperties();
+
   void SetSmartDashboardType(const wpi::Twine& type) override;
   void SetActuator(bool value) override;
   void SetSafeState(std::function<void()> func) override;

wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h

@@ -31,8 +31,8 @@ class SendableChooserBase : public Sendable,
   SendableChooserBase();
   ~SendableChooserBase() override = default;
 
-  SendableChooserBase(SendableChooserBase&&) = default;
-  SendableChooserBase& operator=(SendableChooserBase&&) = default;
+  SendableChooserBase(SendableChooserBase&& oth);
+  SendableChooserBase& operator=(SendableChooserBase&& oth);
 
  protected:
   static constexpr const char* kDefault = "default";

wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h

@@ -10,12 +10,14 @@
 #include <memory>
 #include <string>
 
+#include <networktables/NetworkTable.h>
 #include <wpi/STLExtras.h>
 #include <wpi/Twine.h>
 
 namespace frc {
 
 class Sendable;
+class SendableBuilderImpl;
 
 /**
  * The SendableRegistry class is the public interface for registering sensors
@@ -26,6 +28,8 @@ class SendableRegistry {
   SendableRegistry(const SendableRegistry&) = delete;
   SendableRegistry& operator=(const SendableRegistry&) = delete;
 
+  using UID = size_t;
+
   /**
    * Gets an instance of the SendableRegistry class.
    *
@@ -252,6 +256,60 @@ class SendableRegistry {
    */
   void DisableLiveWindow(Sendable* sendable);
 
+  /**
+   * Get unique id for an object.  Since objects can move, use this instead
+   * of storing Sendable* directly if ownership is in question.
+   *
+   * @param sendable object
+   * @return unique id
+   */
+  UID GetUniqueId(Sendable* sendable);
+
+  /**
+   * Get sendable object for a given unique id.
+   *
+   * @param uid unique id
+   * @return sendable object (may be null)
+   */
+  Sendable* GetSendable(UID uid);
+
+  /**
+   * Publishes an object in the registry to a network table.
+   *
+   * @param sendableUid sendable unique id
+   * @param table network table
+   */
+  void Publish(UID sendableUid, std::shared_ptr<NetworkTable> table);
+
+  /**
+   * Updates network table information from an object.
+   *
+   * @param sendableUid sendable unique id
+   */
+  void Update(UID sendableUid);
+
+  /**
+   * Data passed to ForeachLiveWindow() callback function
+   */
+  struct CallbackData {
+    CallbackData(Sendable* sendable_, wpi::StringRef name_,
+                 wpi::StringRef subsystem_, Sendable* parent_,
+                 std::shared_ptr<void>& data_, SendableBuilderImpl& builder_)
+        : sendable(sendable_),
+          name(name_),
+          subsystem(subsystem_),
+          parent(parent_),
+          data(data_),
+          builder(builder_) {}
+
+    Sendable* sendable;
+    wpi::StringRef name;
+    wpi::StringRef subsystem;
+    Sendable* parent;
+    std::shared_ptr<void>& data;
+    SendableBuilderImpl& builder;
+  };
+
   /**
    * Iterates over LiveWindow-enabled objects in the registry.
    * It is *not* safe to call other SendableRegistry functions from the
@@ -262,10 +320,7 @@ class SendableRegistry {
    */
   void ForeachLiveWindow(
       int dataHandle,
-      wpi::function_ref<void(Sendable* sendable, wpi::StringRef name,
-                             wpi::StringRef subsystem, Sendable* parent,
-                             std::shared_ptr<void>& data)>
-          callback) const;
+      wpi::function_ref<void(CallbackData& cbdata)> callback) const;
 
  private:
   SendableRegistry();

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

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <array>
 #include <utility>
 #include <vector>
 
@@ -43,6 +44,18 @@ class Spline {
 
   virtual ~Spline() = default;
 
+  /**
+   * Represents a control vector for a spline.
+   *
+   * Each element in each array represents the value of the derivative at the
+   * index. For example, the value of x[2] is the second derivative in the x
+   * dimension.
+   */
+  struct ControlVector {
+    std::array<double, (Degree + 1) / 2> x;
+    std::array<double, (Degree + 1) / 2> y;
+  };
+
   /**
    * Gets the pose and curvature at some point t on the spline.
    *

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

@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <array>
+#include <utility>
 #include <vector>
 
 #include "frc/spline/CubicHermiteSpline.h"
@@ -20,39 +22,79 @@ namespace frc {
 class SplineHelper {
  public:
   /**
-   * Returns a set of cubic splines corresponding to the provided waypoints. The
-   * user is free to set the direction of the start and end point. The
-   * directions for the middle waypoints are determined automatically to ensure
-   * continuous curvature throughout the path.
+   * Returns 2 cubic control vectors from a set of exterior waypoints and
+   * interior translations.
+   *
+   * @param start             The starting pose.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end               The ending pose.
+   * @return 2 cubic control vectors.
+   */
+  static std::array<Spline<3>::ControlVector, 2>
+  CubicControlVectorsFromWaypoints(
+      const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+      const Pose2d& end);
+
+  /**
+   * Returns quintic control vectors from a set of waypoints.
+   *
+   * @param waypoints The waypoints
+   * @return List of control vectors
+   */
+  static std::vector<Spline<5>::ControlVector>
+  QuinticControlVectorsFromWaypoints(const std::vector<Pose2d>& waypoints);
+
+  /**
+   * Returns a set of cubic splines corresponding to the provided control
+   * vectors. The user is free to set the direction of the start and end
+   * point. The directions for the middle waypoints are determined
+   * automatically to ensure continuous curvature throughout the path.
    *
    * The derivation for the algorithm used can be found here:
    * <https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08/undervisningsmateriale/chap7alecture.pdf>
    *
-   * @param start The starting waypoint.
-   * @param waypoints The middle waypoints. This can be left blank if you only
-   * wish to create a path with two waypoints.
-   * @param end The ending waypoint.
+   * @param start The starting control vector.
+   * @param waypoints The middle waypoints. This can be left blank if you
+   * only wish to create a path with two waypoints.
+   * @param end The ending control vector.
    *
    * @return A vector of cubic hermite splines that interpolate through the
    * provided waypoints.
    */
-  static std::vector<CubicHermiteSpline> CubicSplinesFromWaypoints(
-      const Pose2d& start, std::vector<Translation2d> waypoints,
-      const Pose2d& end);
+  static std::vector<CubicHermiteSpline> CubicSplinesFromControlVectors(
+      const Spline<3>::ControlVector& start,
+      std::vector<Translation2d> waypoints,
+      const Spline<3>::ControlVector& end);
 
   /**
-   * Returns a set of quintic splines corresponding to the provided waypoints.
-   * The user is free to set the direction of all waypoints. Continuous
+   * Returns a set of quintic splines corresponding to the provided control
+   * vectors. The user is free to set the direction of all waypoints. Continuous
    * curvature is guaranteed throughout the path.
    *
-   * @param waypoints The waypoints.
+   * @param controlVectors The control vectors.
    * @return A vector of quintic hermite splines that interpolate through the
    * provided waypoints.
    */
-  static std::vector<QuinticHermiteSpline> QuinticSplinesFromWaypoints(
-      const std::vector<Pose2d>& waypoints);
+  static std::vector<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
+      const std::vector<Spline<5>::ControlVector>& controlVectors);
 
  private:
+  static Spline<3>::ControlVector CubicControlVector(double scalar,
+                                                     const Pose2d& point) {
+    return {
+        {point.Translation().X().to<double>(), scalar * point.Rotation().Cos()},
+        {point.Translation().Y().to<double>(),
+         scalar * point.Rotation().Sin()}};
+  }
+
+  static Spline<5>::ControlVector QuinticControlVector(double scalar,
+                                                       const Pose2d& point) {
+    return {{point.Translation().X().to<double>(),
+             scalar * point.Rotation().Cos(), 0.0},
+            {point.Translation().Y().to<double>(),
+             scalar * point.Rotation().Sin(), 0.0}};
+  }
+
   /**
    * Thomas algorithm for solving tridiagonal systems Af = d.
    *

wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h

@@ -0,0 +1,140 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * Represents the configuration for generating a trajectory. This class stores
+ * the start velocity, end velocity, max velocity, max acceleration, custom
+ * constraints, and the reversed flag.
+ *
+ * The class must be constructed with a max velocity and max acceleration.
+ * The other parameters (start velocity, end velocity, constraints, reversed)
+ * have been defaulted to reasonable values (0, 0, {}, false). These values can
+ * be changed via the SetXXX methods.
+ */
+class TrajectoryConfig {
+ public:
+  /**
+   * Constructs a config object.
+   * @param maxVelocity The max velocity of the trajectory.
+   * @param maxAcceleration The max acceleration of the trajectory.
+   */
+  TrajectoryConfig(units::meters_per_second_t maxVelocity,
+                   units::meters_per_second_squared_t maxAcceleration)
+      : m_maxVelocity(maxVelocity), m_maxAcceleration(maxAcceleration) {}
+
+  TrajectoryConfig(const TrajectoryConfig&) = delete;
+  TrajectoryConfig& operator=(const TrajectoryConfig&) = delete;
+
+  TrajectoryConfig(TrajectoryConfig&&) = default;
+  TrajectoryConfig& operator=(TrajectoryConfig&&) = default;
+
+  /**
+   * Sets the start velocity of the trajectory.
+   * @param startVelocity The start velocity of the trajectory.
+   */
+  void SetStartVelocity(units::meters_per_second_t startVelocity) {
+    m_startVelocity = startVelocity;
+  }
+
+  /**
+   * Sets the end velocity of the trajectory.
+   * @param endVelocity The end velocity of the trajectory.
+   */
+  void SetEndVelocity(units::meters_per_second_t endVelocity) {
+    m_endVelocity = endVelocity;
+  }
+
+  /**
+   * Sets the reversed flag of the trajectory.
+   * @param reversed Whether the trajectory should be reversed or not.
+   */
+  void SetReversed(bool reversed) { m_reversed = reversed; }
+
+  /**
+   * Adds a user-defined constraint to the trajectory.
+   * @param constraint The user-defined constraint.
+   */
+  template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
+                                     TrajectoryConstraint, Constraint>>>
+  void AddConstraint(Constraint constraint) {
+    m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
+  }
+
+  /**
+   * Adds a differential drive kinematics constraint to ensure that
+   * no wheel velocity of a differential drive goes above the max velocity.
+   *
+   * @param kinematics The differential drive kinematics.
+   */
+  void SetKinematics(const DifferentialDriveKinematics& kinematics) {
+    AddConstraint(
+        DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
+  }
+
+  /**
+   * Returns the starting velocity of the trajectory.
+   * @return The starting velocity of the trajectory.
+   */
+  units::meters_per_second_t StartVelocity() const { return m_startVelocity; }
+
+  /**
+   * Returns the ending velocity of the trajectory.
+   * @return The ending velocity of the trajectory.
+   */
+  units::meters_per_second_t EndVelocity() const { return m_endVelocity; }
+
+  /**
+   * Returns the maximum velocity of the trajectory.
+   * @return The maximum velocity of the trajectory.
+   */
+  units::meters_per_second_t MaxVelocity() const { return m_maxVelocity; }
+
+  /**
+   * Returns the maximum acceleration of the trajectory.
+   * @return The maximum acceleration of the trajectory.
+   */
+  units::meters_per_second_squared_t MaxAcceleration() const {
+    return m_maxAcceleration;
+  }
+
+  /**
+   * Returns the user-defined constraints of the trajectory.
+   * @return The user-defined constraints of the trajectory.
+   */
+  const std::vector<std::unique_ptr<TrajectoryConstraint>>& Constraints()
+      const {
+    return m_constraints;
+  }
+
+  /**
+   * Returns whether the trajectory is reversed or not.
+   * @return whether the trajectory is reversed or not.
+   */
+  bool IsReversed() const { return m_reversed; }
+
+ private:
+  units::meters_per_second_t m_startVelocity = 0_mps;
+  units::meters_per_second_t m_endVelocity = 0_mps;
+  units::meters_per_second_t m_maxVelocity;
+  units::meters_per_second_squared_t m_maxAcceleration;
+  std::vector<std::unique_ptr<TrajectoryConstraint>> m_constraints;
+  bool m_reversed = false;
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h

@@ -13,6 +13,8 @@
 
 #include "frc/spline/SplineParameterizer.h"
 #include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
 
 namespace frc {
@@ -24,63 +26,71 @@ class TrajectoryGenerator {
   using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
 
   /**
-   * Generates a trajectory with the given waypoints and constraints.
+   * Generates a trajectory from the given control vectors and config. This
+   * method uses clamped cubic splines -- a method in which the exterior control
+   * vectors and interior waypoints are provided. The headings are automatically
+   * determined at the interior points to ensure continuous curvature.
    *
-   * @param waypoints A vector of points that the trajectory must go through.
-   * @param constraints A vector of various velocity and acceleration
-   * constraints.
-   * @param startVelocity The start velocity for the trajectory.
-   * @param endVelocity The end velocity for the trajectory.
-   * @param maxVelocity The max velocity for the trajectory.
-   * @param maxAcceleration The max acceleration for the trajectory.
-   * @param reversed Whether the robot should move backwards. Note that the
-   * robot will still move from a -> b -> ... -> z as defined in the waypoints.
-   *
-   * @return The trajectory.
+   * @param initial           The initial control vector.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end               The ending control vector.
+   * @param config            The configuration for the trajectory.
+   * @return The generated trajectory.
    */
   static Trajectory GenerateTrajectory(
-      const std::vector<Pose2d>& waypoints,
-      std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
-      units::meters_per_second_t startVelocity,
-      units::meters_per_second_t endVelocity,
-      units::meters_per_second_t maxVelocity,
-      units::meters_per_second_squared_t maxAcceleration, bool reversed);
+      Spline<3>::ControlVector initial,
+      const std::vector<Translation2d>& interiorWaypoints,
+      Spline<3>::ControlVector end, const TrajectoryConfig& config);
 
   /**
-   * Generates a trajectory with the given waypoints and constraints.
+   * Generates a trajectory from the given waypoints and config. This method
+   * uses clamped cubic splines -- a method in which the initial pose, final
+   * pose, and interior waypoints are provided.  The headings are automatically
+   * determined at the interior points to ensure continuous curvature.
    *
-   * @param start The starting pose for the trajectory.
-   * @param waypoints The interior waypoints for the trajectory. The headings
-   * will be determined automatically to ensure continuous curvature.
-   * @param end The ending pose for the trajectory.
-   * @param constraints A vector of various velocity and acceleration
-   * constraints.
-   * @param startVelocity The start velocity for the trajectory.
-   * @param endVelocity The end velocity for the trajectory.
-   * @param maxVelocity The max velocity for the trajectory.
-   * @param maxAcceleration The max acceleration for the trajectory.
-   * @param reversed Whether the robot should move backwards. Note that the
-   * robot will still move from a -> b -> ... -> z as defined in the waypoints.
-   *
-   * @return The trajectory.
+   * @param start             The starting pose.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end               The ending pose.
+   * @param config            The configuration for the trajectory.
+   * @return The generated trajectory.
    */
   static Trajectory GenerateTrajectory(
-      const Pose2d& start, const std::vector<Translation2d>& waypoints,
-      const Pose2d& end,
-      std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
-      units::meters_per_second_t startVelocity,
-      units::meters_per_second_t endVelocity,
-      units::meters_per_second_t maxVelocity,
-      units::meters_per_second_squared_t maxAcceleration, bool reversed);
+      const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+      const Pose2d& end, const TrajectoryConfig& config);
+
+  /**
+   * Generates a trajectory from the given quintic control vectors and config.
+   * This method uses quintic hermite splines -- therefore, all points must be
+   * represented by control vectors. Continuous curvature is guaranteed in this
+   * method.
+   *
+   * @param controlVectors List of quintic control vectors.
+   * @param config         The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  static Trajectory GenerateTrajectory(
+      std::vector<Spline<5>::ControlVector> controlVectors,
+      const TrajectoryConfig& config);
+
+  /**
+   * Generates a trajectory from the given waypoints and config. This method
+   * uses quintic hermite splines -- therefore, all points must be represented
+   * by Pose2d objects. Continuous curvature is guaranteed in this method.
+   *
+   * @param waypoints List of waypoints..
+   * @param config    The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  static Trajectory GenerateTrajectory(const std::vector<Pose2d>& waypoints,
+                                       const TrajectoryConfig& config);
 
- private:
   /**
    * Generate spline points from a vector of splines by parameterizing the
    * splines.
    *
    * @param splines The splines to parameterize.
    *
-   * @return The spline points.
+   * @return The spline points for use in time parameterization of a trajectory.
    */
   template <typename Spline>
   static std::vector<PoseWithCurvature> SplinePointsFromSplines(

wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h

@@ -67,7 +67,7 @@ class TrajectoryParameterizer {
    */
   static Trajectory TimeParameterizeTrajectory(
       const std::vector<PoseWithCurvature>& points,
-      std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
+      const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
       units::meters_per_second_t startVelocity,
       units::meters_per_second_t endVelocity,
       units::meters_per_second_t maxVelocity,

wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h

@@ -68,6 +68,8 @@ class TrapezoidProfile {
   TrapezoidProfile(Constraints constraints, State goal,
                    State initial = State{0_m, 0_mps});
 
+  TrapezoidProfile(const TrapezoidProfile&) = default;
+  TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
   TrapezoidProfile(TrapezoidProfile&&) = default;
   TrapezoidProfile& operator=(TrapezoidProfile&&) = default;
 

wpilibc/src/main/native/include/frc2/Timer.h

@@ -0,0 +1,138 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/deprecated.h>
+#include <wpi/mutex.h>
+
+#include "frc/Base.h"
+
+namespace frc2 {
+
+/**
+ * Pause the task for a specified time.
+ *
+ * Pause the execution of the program for a specified period of time given in
+ * seconds. Motors will continue to run at their last assigned values, and
+ * sensors will continue to update. Only the task containing the wait will pause
+ * until the wait time is expired.
+ *
+ * @param seconds Length of time to pause, in seconds.
+ */
+void Wait(units::second_t seconds);
+
+/**
+ * @brief  Gives real-time clock system time with nanosecond resolution
+ * @return The time, just in case you want the robot to start autonomous at 8pm
+ *         on Saturday.
+ */
+units::second_t GetTime();
+
+/**
+ * A wrapper for the frc::Timer class that returns unit-typed values.
+ */
+class Timer {
+ public:
+  /**
+   * Create a new timer object.
+   *
+   * Create a new timer object and reset the time to zero. The timer is
+   * initially not running and must be started.
+   */
+  Timer();
+
+  virtual ~Timer() = default;
+
+  Timer(const Timer& rhs);
+  Timer& operator=(const Timer& rhs);
+  Timer(Timer&& rhs);
+  Timer& operator=(Timer&& rhs);
+
+  /**
+   * Get the current time from the timer. If the clock is running it is derived
+   * from the current system clock the start time stored in the timer class. If
+   * the clock is not running, then return the time when it was last stopped.
+   *
+   * @return Current time value for this timer in seconds
+   */
+  units::second_t Get() const;
+
+  /**
+   * Reset the timer by setting the time to 0.
+   *
+   * Make the timer startTime the current time so new requests will be relative
+   * to now.
+   */
+  void Reset();
+
+  /**
+   * Start the timer running.
+   *
+   * Just set the running flag to true indicating that all time requests should
+   * be relative to the system clock.
+   */
+  void Start();
+
+  /**
+   * Stop the timer.
+   *
+   * This computes the time as of now and clears the running flag, causing all
+   * subsequent time requests to be read from the accumulated time rather than
+   * looking at the system clock.
+   */
+  void Stop();
+
+  /**
+   * Check if the period specified has passed and if it has, advance the start
+   * time by that period. This is useful to decide if it's time to do periodic
+   * work without drifting later by the time it took to get around to checking.
+   *
+   * @param period The period to check for.
+   * @return       True if the period has passed.
+   */
+  bool HasPeriodPassed(units::second_t period);
+
+  /**
+   * Return the FPGA system clock time in seconds.
+   *
+   * Return the time from the FPGA hardware clock in seconds since the FPGA
+   * started. Rolls over after 71 minutes.
+   *
+   * @returns Robot running time in seconds.
+   */
+  static units::second_t GetFPGATimestamp();
+
+  /**
+   * Return the approximate match time.
+   *
+   * The FMS does not send an official match time to the robots, but does send
+   * an approximate match time. The value will count down the time remaining in
+   * the current period (auto or teleop).
+   *
+   * Warning: This is not an official time (so it cannot be used to dispute ref
+   * calls or guarantee that a function will trigger before the match ends).
+   *
+   * The Practice Match function of the DS approximates the behavior seen on the
+   * field.
+   *
+   * @return Time remaining in current match period (auto or teleop)
+   */
+  static units::second_t GetMatchTime();
+
+  // The time, in seconds, at which the 32-bit FPGA timestamp rolls over to 0
+  static const units::second_t kRolloverTime;
+
+ private:
+  units::second_t m_startTime = 0_s;
+  units::second_t m_accumulatedTime = 0_s;
+  bool m_running = false;
+  mutable wpi::mutex m_mutex;
+};
+
+}  // namespace frc2

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

@@ -122,7 +122,7 @@ class Command : public frc::ErrorBase {
    * @param condition the interrupt condition
    * @return the command with the interrupt condition added
    */
-  ParallelRaceGroup InterruptOn(std::function<bool()> condition) &&;
+  ParallelRaceGroup WithInterrupt(std::function<bool()> condition) &&;
 
   /**
    * Decorates this command with a runnable to run before this command starts.
@@ -138,7 +138,7 @@ class Command : public frc::ErrorBase {
    * @param toRun the Runnable to run
    * @return the decorated command
    */
-  SequentialCommandGroup WhenFinished(std::function<void()> toRun) &&;
+  SequentialCommandGroup AndThen(std::function<void()> toRun) &&;
 
   /**
    * Decorates this command to run perpetually, ignoring its ordinary end

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

@@ -360,6 +360,13 @@ class CommandScheduler final : public frc::Sendable,
   wpi::SmallVector<Action, 4> m_interruptActions;
   wpi::SmallVector<Action, 4> m_finishActions;
 
+  // Flag and queues for avoiding concurrent modification if commands are
+  // scheduled/canceled during run
+
+  bool m_inRunLoop = false;
+  wpi::DenseMap<Command*, bool> m_toSchedule;
+  wpi::SmallVector<Command*, 4> m_toCancel;
+
   friend class CommandTestBase;
 };
 }  // namespace frc2

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

@@ -19,7 +19,7 @@ namespace frc2 {
  * A command that starts a notifier to run the given runnable periodically in a
  * separate thread. Has no end condition as-is; either subclass it or use {@link
  * Command#withTimeout(double)} or
- * {@link Command#interruptOn(BooleanSupplier)} to give it one.
+ * {@link Command#withInterrupt(BooleanSupplier)} to give it one.
  *
  * <p>WARNING: Do not use this class unless you are confident in your ability to
  * make the executed code thread-safe.  If you do not know what "thread-safe"