Fix "'dimensionless' ambiguous symbol" error (#2382)

A typedef for units::dimensionless::dimensionless is defined, which
conflicted with the namespace when we added "using namespace
dimensionless". This patch reverts the "using namespace" directive.
"using" directives were added to pull three of the four relevant
typedefs but avoid the "dimensionless" type conflict.

This issue was first introduced in #2301.

.github/workflows/gradle-wrapper-validation.yml

@@ -0,0 +1,10 @@
+name: "Validate Gradle Wrapper"
+on: [push]
+
+jobs:
+  validation:
+    name: "Validation"
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+      - uses: gradle/wrapper-validation-action@v1

.styleguide

@@ -11,6 +11,7 @@ cppSrcFileInclude {
 generatedFileExclude {
   FRCNetComm\.java$
   simulation/gz_msgs/src/include/simulation/gz_msgs/msgs\.h$
+  simulation/halsim_gui/src/main/native/include/portable-file-dialogs\.h$
 }
 
 repoRootNameOverride {

.wpilib/wpilib_preferences.json

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

CMakeLists.txt

@@ -55,6 +55,7 @@ option(USE_VCPKG_LIBUV "Use vcpkg libuv" OFF)
 option(USE_VCPKG_EIGEN "Use vcpkg eigen" OFF)
 option(FLAT_INSTALL_WPILIB "Use a flat install directory" OFF)
 option(WITH_SIMULATION_MODULES "build simulation modules" OFF)
+set(OPENCV_JAVA_INSTALL_DIR "" CACHE PATH "Location to search for the OpenCV jar file")
 
 if (NOT WITHOUT_JAVA AND NOT BUILD_SHARED_LIBS)
     message(FATAL_ERROR "
@@ -64,6 +65,15 @@ FATAL: Cannot build static libs with Java enabled.
 ")
 endif()
 
+if (WITHOUT_JAVA OR WITHOUT_CSCORE)
+    if(NOT "${OPENCV_JAVA_INSTALL_DIR}" STREQUAL "")
+        message(WARNING "
+WARNING: OpenCV Java dir set but java is not enabled!
+It will be ignored.
+")
+    endif()
+endif()
+
 set( wpilib_dest wpilib)
 set( include_dest wpilib/include )
 set( main_lib_dest wpilib/lib )

MavenArtifacts.md

@@ -5,8 +5,8 @@ WPILib publishes its built artifacts to our Maven server for use by downstream p
 ## Repositories
 We provide two repositories. These repositories are:
 
-* (Release)     https://first.wpi.edu/FRC/roborio/maven/release/
-* (Development) https://first.wpi.edu/FRC/roborio/maven/development/
+* (Release)     https://frcmaven.wpi.edu/artifactory/release/
+* (Development) https://frcmaven.wpi.edu/artifactory/development/
 
 The release repository is where official WPILib releases are pushed.
 The development repository is where development releases of every commit to [master](https://github.com/wpilibsuite/allwpilib/tree/master) is pushed.
@@ -72,6 +72,10 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
 * hal
   * wpiutil
 
+* halsim
+  * imgui
+  * wpiutil
+
 * ntcore
   * wpiutil
 
@@ -85,7 +89,6 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
   * opencv
   * wpiutil
 
-
 * wpilibj
   * hal
   * cameraserver
@@ -93,10 +96,35 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
   * cscore
   * wpiutil
 
-
 * wpilibc
   * hal
   * cameraserver
   * ntcore
   * cscore
   * wpiutil
+
+* wpilibNewCommands
+  * wpilibc
+  * hal
+  * cameraserver
+  * ntcore
+  * cscore
+  * wpiutil
+
+* wpilibNewCommands
+  * wpilibc
+  * hal
+  * cameraserver
+  * ntcore
+  * cscore
+  * wpiutil
+
+### Third Party Artifacts
+
+This repository provides the builds of the following third party software.
+
+All artifacts are based at `edu.wpi.first.thirdparty.frcYEAR` in the repository.
+
+* googletest
+* imgui
+* opencv

README-CMAKE.md

@@ -34,6 +34,8 @@ The following build options are available:
   * TODO
 * EXTERNAL_HAL_FILE
   * TODO
+* OPENCV_JAVA_INSTALL_DIR
+  * Set this option to the location of the archive of the OpenCV Java bindings (it should be called opencv-xxx.jar, with the x'es being version numbers). NOTE: set it to the LOCATION of the file, not the file itself!
 
 ## Build Setup
 

README.md

@@ -27,7 +27,7 @@ Using Gradle makes building WPILib very straightforward. It only has a few depen
     - On Linux, GCC works fine
     - On Windows, you need Visual Studio 2019 (the free community edition works fine).
       Make sure to select the C++ Programming Language for installation
-- [ARM Compiler Toolchain](https://github.com/wpilibsuite/toolchain-builder/releases)
+- [ARM Compiler Toolchain](https://github.com/wpilibsuite/roborio-toolchain/releases)
   * Note that for 2020 and beyond, you should use version 7 or greater of GCC
 - Doxygen (Only required if you want to build the C++ documentation)
 

ThirdPartyNotices.txt

@@ -44,6 +44,7 @@ Team 254 Library      wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineP
                       wpilibc/src/main/native/include/spline/SplineParameterizer.h
                       wpilibc/src/main/native/include/trajectory/TrajectoryParameterizer.h
                       wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
+Portable File Dialogs simulation/halsim_gui/src/main/native/include/portable-file-dialogs.h
 
 
 ==============================================================================

cscore/CMakeLists.txt

@@ -69,7 +69,9 @@ if (NOT WITHOUT_JAVA)
 
     #find java files, copy them locally
 
-    set(OPENCV_JAVA_INSTALL_DIR ${OpenCV_INSTALL_PATH}/share/OpenCV/java/)
+    if("${OPENCV_JAVA_INSTALL_DIR}" STREQUAL "")
+        set(OPENCV_JAVA_INSTALL_DIR ${OpenCV_INSTALL_PATH}/share/OpenCV/java/)
+    endif()
 
     find_file(OPENCV_JAR_FILE NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin NO_DEFAULT_PATH)
     find_file(OPENCV_JNI_FILE NAMES libopencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.so

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -102,7 +102,7 @@ static bool IsPercentageProperty(wpi::StringRef name) {
   if (name.startswith("raw_")) name = name.substr(4);
   return name == "brightness" || name == "contrast" || name == "saturation" ||
          name == "hue" || name == "sharpness" || name == "gain" ||
-         name == "exposure_absolute";
+         name == "exposure_absolute" || name == "exposure_time_absolute";
 }
 
 static constexpr const int quirkLifeCamHd3000[] = {
@@ -112,6 +112,11 @@ static constexpr char const* quirkPS3EyePropExAuto = "auto_exposure";
 static constexpr char const* quirkPS3EyePropExValue = "exposure";
 static constexpr const int quirkPS3EyePropExAutoOn = 0;
 static constexpr const int quirkPS3EyePropExAutoOff = 1;
+static constexpr char const* quirkPiCameraPropExAuto = "auto_exposure";
+static constexpr char const* quirkPiCameraPropExValue =
+    "exposure_time_absolute";
+static constexpr const int quirkPiCameraPropExAutoOn = 0;
+static constexpr const int quirkPiCameraPropExAutoOff = 1;
 
 int UsbCameraImpl::RawToPercentage(const UsbCameraProperty& rawProp,
                                    int rawValue) {
@@ -1112,6 +1117,25 @@ void UsbCameraImpl::DeviceCacheVideoModes() {
     }
   }
 
+  // The Pi camera reports mode ranges, which we don't currently handle, so only
+  // provide a set of discrete modes; list based on
+  // https://picamera.readthedocs.io/en/release-1.10/fov.html
+  if (modes.empty() && m_picamera) {
+    for (VideoMode::PixelFormat pixelFormat :
+         {VideoMode::kYUYV, VideoMode::kMJPEG, VideoMode::kBGR}) {
+      modes.emplace_back(pixelFormat, 1920, 1080, 30);
+      modes.emplace_back(pixelFormat, 2592, 1944, 15);
+      modes.emplace_back(pixelFormat, 1296, 972, 42);
+      modes.emplace_back(pixelFormat, 1296, 730, 49);
+      modes.emplace_back(pixelFormat, 640, 480, 90);
+      modes.emplace_back(pixelFormat, 320, 240, 90);
+      modes.emplace_back(pixelFormat, 160, 120, 90);
+      modes.emplace_back(pixelFormat, 640, 480, 60);
+      modes.emplace_back(pixelFormat, 320, 240, 60);
+      modes.emplace_back(pixelFormat, 160, 120, 60);
+    }
+  }
+
   {
     std::scoped_lock lock(m_mutex);
     m_videoModes.swap(modes);
@@ -1192,6 +1216,7 @@ void UsbCameraImpl::SetQuirks() {
   wpi::StringRef desc = GetDescription(descbuf);
   m_lifecam_exposure =
       desc.endswith("LifeCam HD-3000") || desc.endswith("LifeCam Cinema (TM)");
+  m_picamera = desc.startswith("mmal service");
 
   int deviceNum = GetDeviceNum(m_path.c_str());
   if (deviceNum >= 0) {
@@ -1248,7 +1273,9 @@ void UsbCameraImpl::SetExposureAuto(CS_Status* status) {
   if (m_ps3eyecam_exposure) {
     SetProperty(GetPropertyIndex(quirkPS3EyePropExAuto),
                 quirkPS3EyePropExAutoOn, status);
-
+  } else if (m_picamera) {
+    SetProperty(GetPropertyIndex(quirkPiCameraPropExAuto),
+                quirkPiCameraPropExAutoOn, status);
   } else {
     SetProperty(GetPropertyIndex(kPropExAuto), 3, status);
   }
@@ -1258,6 +1285,9 @@ void UsbCameraImpl::SetExposureHoldCurrent(CS_Status* status) {
   if (m_ps3eyecam_exposure) {
     SetProperty(GetPropertyIndex(quirkPS3EyePropExAuto),
                 quirkPS3EyePropExAutoOff, status);  // manual
+  } else if (m_picamera) {
+    SetProperty(GetPropertyIndex(quirkPiCameraPropExAuto),
+                quirkPiCameraPropExAutoOff, status);  // manual
   } else {
     SetProperty(GetPropertyIndex(kPropExAuto), 1, status);  // manual
   }
@@ -1267,6 +1297,9 @@ void UsbCameraImpl::SetExposureManual(int value, CS_Status* status) {
   if (m_ps3eyecam_exposure) {
     SetProperty(GetPropertyIndex(quirkPS3EyePropExAuto),
                 quirkPS3EyePropExAutoOff, status);  // manual
+  } else if (m_picamera) {
+    SetProperty(GetPropertyIndex(quirkPiCameraPropExAuto),
+                quirkPiCameraPropExAutoOff, status);  // manual
   } else {
     SetProperty(GetPropertyIndex(kPropExAuto), 1, status);  // manual
   }
@@ -1277,6 +1310,8 @@ void UsbCameraImpl::SetExposureManual(int value, CS_Status* status) {
   }
   if (m_ps3eyecam_exposure) {
     SetProperty(GetPropertyIndex(quirkPS3EyePropExValue), value, status);
+  } else if (m_picamera) {
+    SetProperty(GetPropertyIndex(quirkPiCameraPropExValue), value, status);
   } else {
     SetProperty(GetPropertyIndex(kPropExValue), value, status);
   }

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -166,6 +166,7 @@ class UsbCameraImpl : public SourceImpl {
   // Quirks
   bool m_lifecam_exposure{false};    // Microsoft LifeCam exposure
   bool m_ps3eyecam_exposure{false};  // PS3 Eyecam exposure
+  bool m_picamera{false};            // Raspberry Pi camera
 
   //
   // Variables protected by m_mutex

docs/build.gradle

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

hal/CMakeLists.txt

@@ -57,7 +57,7 @@ set_property(TARGET hal PROPERTY FOLDER "libraries")
 
 install(TARGETS hal EXPORT hal DESTINATION "${main_lib_dest}")
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/hal")
-install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/gen DESTINATION "${include_dest}/hal")
+install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/gen/ DESTINATION "${include_dest}/hal")
 
 if (MSVC OR FLAT_INSTALL_WPILIB)
     set (hal_config_dir ${wpilib_dest})

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -130,6 +130,8 @@ public final class HAL extends JNIWrapper {
                                      String details, String location, String callStack,
                                      boolean printMsg);
 
+  public static native int sendConsoleLine(String line);
+
   public static native int getPortWithModule(byte module, byte channel);
 
   public static native int getPort(byte channel);

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -38,4 +38,6 @@ public class InterruptJNI extends JNIWrapper {
 
   public static native void setInterruptUpSourceEdge(int interruptHandle, boolean risingEdge,
                                                      boolean fallingEdge);
+
+  public static native void releaseWaitingInterrupt(int interruptHandle);
 }

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

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

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -75,6 +75,9 @@ void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle) {
   dutyCycleHandles->Free(dutyCycleHandle);
 }
 
+void HAL_SetDutyCycleSimDevice(HAL_EncoderHandle handle,
+                               HAL_SimDeviceHandle device) {}
+
 int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
                                   int32_t* status) {
   auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -176,27 +176,27 @@ int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
 
     if (baseLength + detailsRef.size() + locationRef.size() +
             callStackRef.size() <=
-        65536) {
+        65535) {
       // Pass through
       retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode,
                                                   details, location, callStack);
-    } else if (baseLength + detailsRef.size() > 65536) {
+    } else if (baseLength + detailsRef.size() > 65535) {
       // Details too long, cut both location and stack
-      auto newLen = 65536 - baseLength;
+      auto newLen = 65535 - baseLength;
       std::string newDetails{details, newLen};
       char empty = '\0';
       retval = FRC_NetworkCommunication_sendError(
           isError, errorCode, isLVCode, newDetails.c_str(), &empty, &empty);
-    } else if (baseLength + detailsRef.size() + locationRef.size() > 65536) {
+    } else if (baseLength + detailsRef.size() + locationRef.size() > 65535) {
       // Location too long, cut stack
-      auto newLen = 65536 - baseLength - detailsRef.size();
+      auto newLen = 65535 - baseLength - detailsRef.size();
       std::string newLocation{location, newLen};
       char empty = '\0';
       retval = FRC_NetworkCommunication_sendError(
           isError, errorCode, isLVCode, details, newLocation.c_str(), &empty);
     } else {
       // Stack too long
-      auto newLen = 65536 - baseLength - detailsRef.size() - locationRef.size();
+      auto newLen = 65535 - baseLength - detailsRef.size() - locationRef.size();
       std::string newCallStack{callStack, newLen};
       retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode,
                                                   details, location,
@@ -229,6 +229,18 @@ int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
   return retval;
 }
 
+int32_t HAL_SendConsoleLine(const char* line) {
+  wpi::StringRef lineRef{line};
+  if (lineRef.size() <= 65535) {
+    // Send directly
+    return FRC_NetworkCommunication_sendConsoleLine(line);
+  } else {
+    // Need to truncate
+    std::string newLine{line, 65535};
+    return FRC_NetworkCommunication_sendConsoleLine(newLine.c_str());
+  }
+}
+
 int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
   return HAL_GetControlWordInternal(controlWord);
 }

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -24,6 +24,7 @@
 #include <wpi/timestamp.h>
 
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "ctre/ctre.h"
 #include "hal/ChipObject.h"
 #include "hal/DriverStation.h"
@@ -78,6 +79,15 @@ void InitializeHAL() {
   InitializeThreads();
 }
 }  // namespace init
+
+void ReleaseFPGAInterrupt(int32_t interruptNumber) {
+  if (!global) {
+    return;
+  }
+  int32_t status = 0;
+  global->writeInterruptForceNumber(static_cast<unsigned char>(interruptNumber),
+                                    &status);
+}
 }  // namespace hal
 
 extern "C" {

hal/src/main/native/athena/HALInternal.h

@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <stdint.h>
+
+namespace hal {
+void ReleaseFPGAInterrupt(int32_t interruptNumber);
+
+}  // namespace hal

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,9 +13,11 @@
 
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/ChipObject.h"
 #include "hal/Errors.h"
+#include "hal/HALBase.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/LimitedHandleResource.h"
 
@@ -268,4 +270,19 @@ void HAL_SetInterruptUpSourceEdge(HAL_InterruptHandle interruptHandle,
   anInterrupt->anInterrupt->writeConfig_FallingEdge(fallingEdge, status);
 }
 
+void HAL_ReleaseWaitingInterrupt(HAL_InterruptHandle interruptHandle,
+                                 int32_t* status) {
+  auto anInterrupt = interruptHandles->Get(interruptHandle);
+  if (anInterrupt == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  uint32_t interruptIndex =
+      static_cast<uint32_t>(getHandleIndex(interruptHandle));
+
+  hal::ReleaseFPGAInterrupt(interruptIndex);
+  hal::ReleaseFPGAInterrupt(interruptIndex + 8);
+}
+
 }  // extern "C"

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -448,6 +448,21 @@ Java_edu_wpi_first_hal_HAL_sendError
   return returnValue;
 }
 
+/*
+ * Class:     edu_wpi_first_hal_HAL
+ * Method:    sendConsoleLine
+ * Signature: (Ljava/lang/String;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_sendConsoleLine
+  (JNIEnv* env, jclass, jstring line)
+{
+  JStringRef lineStr{env, line};
+
+  jint returnValue = HAL_SendConsoleLine(lineStr.c_str());
+  return returnValue;
+}
+
 /*
  * Class:     edu_wpi_first_hal_HAL
  * Method:    getPortWithModule

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -294,4 +294,19 @@ Java_edu_wpi_first_hal_InterruptJNI_setInterruptUpSourceEdge
   CheckStatus(env, status);
 }
 
+/*
+ * Class:     edu_wpi_first_hal_InterruptJNI
+ * Method:    releaseWaitingInterrupt
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_InterruptJNI_releaseWaitingInterrupt
+  (JNIEnv* env, jclass, jint interruptHandle)
+{
+  int32_t status = 0;
+  HAL_ReleaseWaitingInterrupt((HAL_InterruptHandle)interruptHandle, &status);
+
+  CheckStatus(env, status);
+}
+
 }  // extern "C"

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2013-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2013-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -37,6 +37,12 @@ extern "C" {
 int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
                       const char* details, const char* location,
                       const char* callStack, HAL_Bool printMsg);
+/**
+ * Sends a line to the driver station console.
+ *
+ * @param line the line to send (null terminated)
+ */
+int32_t HAL_SendConsoleLine(const char* line);
 
 /**
  * Gets the current control word of the driver station.

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -153,6 +153,16 @@ void HAL_AttachInterruptHandlerThreaded(HAL_InterruptHandle interruptHandle,
 void HAL_SetInterruptUpSourceEdge(HAL_InterruptHandle interruptHandle,
                                   HAL_Bool risingEdge, HAL_Bool fallingEdge,
                                   int32_t* status);
+
+/**
+ * Releases a waiting interrupt.
+ *
+ * This will release both rising and falling waiters.
+ *
+ * @param interruptHandle the interrupt handle to release
+ */
+void HAL_ReleaseWaitingInterrupt(HAL_InterruptHandle interruptHandle,
+                                 int32_t* status);
 #ifdef __cplusplus
 }  // extern "C"
 #endif

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -24,4 +24,7 @@ typedef int32_t (*HALSIM_SendErrorHandler)(
     const char* location, const char* callStack, HAL_Bool printMsg);
 void HALSIM_SetSendError(HALSIM_SendErrorHandler handler);
 
+typedef int32_t (*HALSIM_SendConsoleLineHandler)(const char* line);
+void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler);
+
 }  // extern "C"

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,6 +18,7 @@
 
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
+#include <wpi/raw_ostream.h>
 
 #include "HALInitializer.h"
 #include "mockdata/DriverStationDataInternal.h"
@@ -29,6 +30,8 @@ static wpi::mutex newDSDataAvailableMutex;
 static int newDSDataAvailableCounter{0};
 static std::atomic_bool isFinalized{false};
 static std::atomic<HALSIM_SendErrorHandler> sendErrorHandler{nullptr};
+static std::atomic<HALSIM_SendConsoleLineHandler> sendConsoleLineHandler{
+    nullptr};
 
 namespace hal {
 namespace init {
@@ -47,6 +50,10 @@ void HALSIM_SetSendError(HALSIM_SendErrorHandler handler) {
   sendErrorHandler.store(handler);
 }
 
+void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler) {
+  sendConsoleLineHandler.store(handler);
+}
+
 int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
                       const char* details, const char* location,
                       const char* callStack, HAL_Bool printMsg) {
@@ -105,6 +112,16 @@ int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
   return retval;
 }
 
+int32_t HAL_SendConsoleLine(const char* line) {
+  auto handler = sendConsoleLineHandler.load();
+  if (handler) {
+    return handler(line);
+  }
+  wpi::outs() << line << "\n";
+  wpi::outs().flush();
+  return 0;
+}
+
 int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
   controlWord->enabled = SimDriverStationData->enabled;
   controlWord->autonomous = SimDriverStationData->autonomous;
@@ -230,6 +247,10 @@ static int& GetThreadLocalLastCount() {
   return lastCount;
 }
 
+void HAL_WaitForCachedControlData(void) {
+  HAL_WaitForCachedControlDataTimeout(0);
+}
+
 HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) {
   int& lastCount = GetThreadLocalLastCount();
   std::unique_lock lock(newDSDataAvailableMutex);

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -566,4 +566,9 @@ void HAL_SetInterruptUpSourceEdge(HAL_InterruptHandle interruptHandle,
   interrupt->fireOnDown = fallingEdge;
   interrupt->fireOnUp = risingEdge;
 }
+
+void HAL_ReleaseWaitingInterrupt(HAL_InterruptHandle interruptHandle,
+                                 int32_t* status) {
+  // Requires a fairly large rewrite to get working
+}
 }  // extern "C"

hal/src/main/native/sim/PortsInternal.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,7 +17,7 @@ constexpr int32_t kNumAnalogOutputs = 2;
 constexpr int32_t kNumCounters = 8;
 constexpr int32_t kNumDigitalHeaders = 10;
 constexpr int32_t kNumPWMHeaders = 10;
-constexpr int32_t kNumDigitalChannels = 26;
+constexpr int32_t kNumDigitalChannels = 31;
 constexpr int32_t kNumPWMChannels = 20;
 constexpr int32_t kNumDigitalPWMOutputs = 6;
 constexpr int32_t kNumEncoders = 8;

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -462,7 +462,23 @@ Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setSendError
     HALSIM_SetSendError([](HAL_Bool isError, int32_t errorCode,
                            HAL_Bool isLVCode, const char* details,
                            const char* location, const char* callStack,
-                           HAL_Bool printMsg) { return 1; });
+                           HAL_Bool printMsg) { return 0; });
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
+ * Method:    setSendConsoleLine
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setSendConsoleLine
+  (JNIEnv*, jclass, jboolean shouldSend)
+{
+  if (shouldSend) {
+    HALSIM_SetSendConsoleLine(nullptr);
+  } else {
+    HALSIM_SetSendConsoleLine([](const char* line) { return 0; });
   }
 }
 

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -22,17 +22,17 @@ void InitializeRoboRioData() {
 RoboRioData* hal::SimRoboRioData;
 void RoboRioData::ResetData() {
   fpgaButton.Reset(false);
-  vInVoltage.Reset(0.0);
+  vInVoltage.Reset(12.0);
   vInCurrent.Reset(0.0);
   userVoltage6V.Reset(6.0);
   userCurrent6V.Reset(0.0);
-  userActive6V.Reset(false);
+  userActive6V.Reset(true);
   userVoltage5V.Reset(5.0);
   userCurrent5V.Reset(0.0);
-  userActive5V.Reset(false);
+  userActive5V.Reset(true);
   userVoltage3V3.Reset(3.3);
   userCurrent3V3.Reset(0.0);
-  userActive3V3.Reset(false);
+  userActive3V3.Reset(true);
   userFaults6V.Reset(0);
   userFaults5V.Reset(0);
   userFaults3V3.Reset(0);

hal/src/main/native/sim/mockdata/RoboRioDataInternal.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -30,22 +30,22 @@ class RoboRioData {
 
  public:
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetFPGAButtonName> fpgaButton{false};
-  SimDataValue<double, HAL_MakeDouble, GetVInVoltageName> vInVoltage{0.0};
+  SimDataValue<double, HAL_MakeDouble, GetVInVoltageName> vInVoltage{12.0};
   SimDataValue<double, HAL_MakeDouble, GetVInCurrentName> vInCurrent{0.0};
   SimDataValue<double, HAL_MakeDouble, GetUserVoltage6VName> userVoltage6V{6.0};
   SimDataValue<double, HAL_MakeDouble, GetUserCurrent6VName> userCurrent6V{0.0};
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetUserActive6VName> userActive6V{
-      false};
+      true};
   SimDataValue<double, HAL_MakeDouble, GetUserVoltage5VName> userVoltage5V{5.0};
   SimDataValue<double, HAL_MakeDouble, GetUserCurrent5VName> userCurrent5V{0.0};
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetUserActive5VName> userActive5V{
-      false};
+      true};
   SimDataValue<double, HAL_MakeDouble, GetUserVoltage3V3Name> userVoltage3V3{
       3.3};
   SimDataValue<double, HAL_MakeDouble, GetUserCurrent3V3Name> userCurrent3V3{
       0.0};
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetUserActive3V3Name> userActive3V3{
-      false};
+      true};
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults6VName> userFaults6V{0};
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults5VName> userFaults5V{0};
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults3V3Name> userFaults3V3{0};

imgui/CMakeLists.txt

@@ -32,6 +32,11 @@ file(GENERATE OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.h
 set_source_files_properties(${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.cpp
     PROPERTIES OBJECT_DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/ProggyDotted.inc)
 
+# stb_image
+file(GENERATE OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/stb_image.cpp
+    CONTENT "#define STBI_WINDOWS_UTF8\n#define STB_IMAGE_IMPLEMENTATION\n#include \"stb_image.h\"\n"
+)
+
 # Add imgui directly to our build.
 set(SAVE_BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS})
 set(BUILD_SHARED_LIBS OFF)
@@ -46,8 +51,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 ${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.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 ${CMAKE_CURRENT_BINARY_DIR}/stb_image.cpp)
 target_link_libraries(imgui PUBLIC gl3w glfw)
-target_include_directories(imgui PUBLIC "$<BUILD_INTERFACE:${imgui_srcdir}>" "$<BUILD_INTERFACE:${imgui_srcdir}/examples>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>")
+target_include_directories(imgui PUBLIC "$<BUILD_INTERFACE:${imgui_srcdir}>" "$<BUILD_INTERFACE:${imgui_srcdir}/examples>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/stb-src>")
 
 set_property(TARGET imgui PROPERTY POSITION_INDEPENDENT_CODE ON)

imgui/CMakeLists.txt.in

@@ -41,3 +41,13 @@ ExternalProject_Add(proggyfonts
   INSTALL_COMMAND   ""
   TEST_COMMAND      ""
 )
+ExternalProject_Add(stb
+  GIT_REPOSITORY    https://github.com/nothings/stb.git
+  GIT_TAG           f67165c2bb2af3060ecae7d20d6f731173485ad0
+  SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/stb-src"
+  BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/stb-build"
+  CONFIGURE_COMMAND ""
+  BUILD_COMMAND     ""
+  INSTALL_COMMAND   ""
+  TEST_COMMAND      ""
+)

shared/config.gradle

@@ -11,7 +11,7 @@ nativeUtils {
       niLibVersion = "2020.10.1"
       opencvVersion = "3.4.7-2"
       googleTestVersion = "1.9.0-4-437e100-1"
-      imguiVersion = "1.72b-2"
+      imguiVersion = "1.72b-3"
     }
   }
 }
@@ -21,6 +21,10 @@ nativeUtils.wpi.addWarningsAsErrors()
 
 nativeUtils.setSinglePrintPerPlatform()
 
+nativeUtils.platformConfigs.named("osxx86-64").configure {
+    it.linker.args << "-headerpad_max_install_names"
+}
+
 model {
     components {
         all {

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,10 +7,6 @@
 
 #include "AddressableLEDGui.h"
 
-#include <cstdio>
-#include <cstring>
-#include <vector>
-
 #include <hal/Ports.h>
 #include <imgui.h>
 #include <imgui_internal.h>
@@ -20,77 +16,55 @@
 
 #include "ExtraGuiWidgets.h"
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
 namespace {
-struct LEDDisplaySettings {
+struct LEDDisplayInfo {
   int numColumns = 10;
   LEDConfig config;
+
+  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
+  void WriteIni(ImGuiTextBuffer* out);
 };
 }  // namespace
 
-static std::vector<LEDDisplaySettings> displaySettings;
+static IniSaver<LEDDisplayInfo> gDisplaySettings{"AddressableLED"};
 
-// read/write columns setting to ini file
-static void* AddressableLEDReadOpen(ImGuiContext* ctx,
-                                    ImGuiSettingsHandler* handler,
-                                    const char* name) {
-  int num;
-  if (wpi::StringRef{name}.getAsInteger(10, num)) return nullptr;
-  if (num < 0) return nullptr;
-  if (num >= static_cast<int>(displaySettings.size()))
-    displaySettings.resize(num + 1);
-  return &displaySettings[num];
-}
-
-static void AddressableLEDReadLine(ImGuiContext* ctx,
-                                   ImGuiSettingsHandler* handler, void* entry,
-                                   const char* lineStr) {
-  auto* settings = static_cast<LEDDisplaySettings*>(entry);
-  // format: columns=#
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
+bool LEDDisplayInfo::ReadIni(wpi::StringRef name, wpi::StringRef value) {
   if (name == "columns") {
     int num;
-    if (value.getAsInteger(10, num)) return;
-    settings->numColumns = num;
+    if (value.getAsInteger(10, num)) return true;
+    numColumns = num;
   } else if (name == "serpentine") {
     int num;
-    if (value.getAsInteger(10, num)) return;
-    settings->config.serpentine = num != 0;
+    if (value.getAsInteger(10, num)) return true;
+    config.serpentine = num != 0;
   } else if (name == "order") {
     int num;
-    if (value.getAsInteger(10, num)) return;
-    settings->config.order = static_cast<LEDConfig::Order>(num);
+    if (value.getAsInteger(10, num)) return true;
+    config.order = static_cast<LEDConfig::Order>(num);
   } else if (name == "start") {
     int num;
-    if (value.getAsInteger(10, num)) return;
-    settings->config.start = static_cast<LEDConfig::Start>(num);
+    if (value.getAsInteger(10, num)) return true;
+    config.start = static_cast<LEDConfig::Start>(num);
+  } else {
+    return false;
   }
+  return true;
 }
 
-static void AddressableLEDWriteAll(ImGuiContext* ctx,
-                                   ImGuiSettingsHandler* handler,
-                                   ImGuiTextBuffer* out_buf) {
-  for (size_t i = 0; i < displaySettings.size(); ++i) {
-    out_buf->appendf(
-        "[AddressableLED][%d]\ncolumns=%d\nserpentine=%d\norder=%d\n"
-        "start=%d\n\n",
-        static_cast<int>(i), displaySettings[i].numColumns,
-        displaySettings[i].config.serpentine ? 1 : 0,
-        static_cast<int>(displaySettings[i].config.order),
-        static_cast<int>(displaySettings[i].config.start));
-  }
+void LEDDisplayInfo::WriteIni(ImGuiTextBuffer* out) {
+  out->appendf("columns=%d\nserpentine=%d\norder=%d\nstart=%d\n", numColumns,
+               config.serpentine ? 1 : 0, static_cast<int>(config.order),
+               static_cast<int>(config.start));
 }
 
 static void DisplayAddressableLEDs() {
   bool hasAny = false;
   static const int numLED = HAL_GetNumAddressableLEDs();
-  if (numLED > static_cast<int>(displaySettings.size()))
-    displaySettings.resize(numLED);
 
   for (int i = 0; i < numLED; ++i) {
     if (!HALSIM_GetAddressableLEDInitialized(i)) continue;
@@ -101,26 +75,27 @@ static void DisplayAddressableLEDs() {
     static HAL_AddressableLEDData data[HAL_kAddressableLEDMaxLength];
     int length = HALSIM_GetAddressableLEDData(i, data);
     bool running = HALSIM_GetAddressableLEDRunning(i);
+    auto& info = gDisplaySettings[i];
 
     ImGui::PushItemWidth(ImGui::GetFontSize() * 6);
     ImGui::LabelText("Length", "%d", length);
     ImGui::LabelText("Running", "%s", running ? "Yes" : "No");
-    ImGui::InputInt("Columns", &displaySettings[i].numColumns);
+    ImGui::InputInt("Columns", &info.numColumns);
     {
       static const char* options[] = {"Row Major", "Column Major"};
-      int val = displaySettings[i].config.order;
+      int val = info.config.order;
       if (ImGui::Combo("Order", &val, options, 2))
-        displaySettings[i].config.order = static_cast<LEDConfig::Order>(val);
+        info.config.order = static_cast<LEDConfig::Order>(val);
     }
     {
       static const char* options[] = {"Upper Left", "Lower Left", "Upper Right",
                                       "Lower Right"};
-      int val = displaySettings[i].config.start;
+      int val = info.config.start;
       if (ImGui::Combo("Start", &val, options, 4))
-        displaySettings[i].config.start = static_cast<LEDConfig::Start>(val);
+        info.config.start = static_cast<LEDConfig::Start>(val);
     }
-    ImGui::Checkbox("Serpentine", &displaySettings[i].config.serpentine);
-    if (displaySettings[i].numColumns < 1) displaySettings[i].numColumns = 1;
+    ImGui::Checkbox("Serpentine", &info.config.serpentine);
+    if (info.numColumns < 1) info.numColumns = 1;
     ImGui::PopItemWidth();
 
     // show as LED indicators
@@ -137,22 +112,13 @@ static void DisplayAddressableLEDs() {
       }
     }
 
-    DrawLEDs(values, length, displaySettings[i].numColumns, colors, 0, 0,
-             displaySettings[i].config);
+    DrawLEDs(values, length, info.numColumns, colors, 0, 0, info.config);
   }
   if (!hasAny) ImGui::Text("No addressable LEDs");
 }
 
 void AddressableLEDGui::Initialize() {
-  // hook ini handler to save columns settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = "AddressableLED";
-  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
-  iniHandler.ReadOpenFn = AddressableLEDReadOpen;
-  iniHandler.ReadLineFn = AddressableLEDReadLine;
-  iniHandler.WriteAllFn = AddressableLEDWriteAll;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-
+  gDisplaySettings.Initialize();
   HALSimGui::AddWindow("Addressable LEDs", DisplayAddressableLEDs,
                        ImGuiWindowFlags_AlwaysAutoResize);
   HALSimGui::SetWindowVisibility("Addressable LEDs", HALSimGui::kHide);

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,6 @@
 
 #include "AnalogInputGui.h"
 
-#include <cstdio>
-
 #include <hal/Ports.h>
 #include <imgui.h>
 #include <mockdata/AnalogGyroData.h>
@@ -16,9 +14,13 @@
 #include <mockdata/SimDeviceData.h>
 
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
+static IniSaver<NameInfo> gAnalogInputs{"AnalogInput"};  // indexed by channel
+
 static void DisplayAnalogInputs() {
   ImGui::Text("(Use Ctrl+Click to edit value)");
   bool hasInputs = false;
@@ -36,8 +38,11 @@ static void DisplayAnalogInputs() {
         first = false;
       }
 
-      char name[32];
-      std::snprintf(name, sizeof(name), "In[%d]", i);
+      auto& info = gAnalogInputs[i];
+      // build name
+      char name[128];
+      info.GetName(name, sizeof(name), "In", i);
+
       if (i < numAccum && HALSIM_GetAnalogGyroInitialized(i)) {
         ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
         ImGui::LabelText(name, "AnalogGyro[%d]", i);
@@ -51,12 +56,16 @@ static void DisplayAnalogInputs() {
         if (ImGui::SliderFloat(name, &val, 0.0, 5.0))
           HALSIM_SetAnalogInVoltage(i, val);
       }
+
+      // context menu to change name
+      info.PopupEditName(i);
     }
   }
   if (!hasInputs) ImGui::Text("No analog inputs");
 }
 
 void AnalogInputGui::Initialize() {
+  gAnalogInputs.Initialize();
   HALSimGui::AddWindow("Analog Inputs", DisplayAnalogInputs,
                        ImGuiWindowFlags_AlwaysAutoResize);
   HALSimGui::SetDefaultWindowPos("Analog Inputs", 640, 20);

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,18 +7,18 @@
 
 #include "AnalogOutGui.h"
 
-#include <cstdio>
-#include <cstring>
-#include <memory>
-
 #include <hal/Ports.h>
 #include <imgui.h>
 #include <mockdata/AnalogOutData.h>
 
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 #include "SimDeviceGui.h"
 
 using namespace halsimgui;
 
+static IniSaver<NameInfo> gAnalogOuts{"AnalogOut"};  // indexed by channel
+
 static void DisplayAnalogOutputs() {
   static const int numAnalog = HAL_GetNumAnalogOutputs();
   static auto init = std::make_unique<bool[]>(numAnalog);
@@ -34,14 +34,20 @@ static void DisplayAnalogOutputs() {
   if (SimDeviceGui::StartDevice("Analog Outputs")) {
     for (int i = 0; i < numAnalog; ++i) {
       if (!init[i]) continue;
-      char name[32];
-      std::snprintf(name, sizeof(name), "Out[%d]", i);
+
+      auto& info = gAnalogOuts[i];
+      char name[128];
+      info.GetName(name, sizeof(name), "Out", i);
       HAL_Value value = HAL_MakeDouble(HALSIM_GetAnalogOutVoltage(i));
       SimDeviceGui::DisplayValue(name, true, &value);
+      info.PopupEditName(i);
     }
 
     SimDeviceGui::FinishDevice();
   }
 }
 
-void AnalogOutGui::Initialize() { SimDeviceGui::Add(DisplayAnalogOutputs); }
+void AnalogOutGui::Initialize() {
+  gAnalogOuts.Initialize();
+  SimDeviceGui::Add(DisplayAnalogOutputs);
+}

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,10 +7,6 @@
 
 #include "DIOGui.h"
 
-#include <cstdio>
-#include <cstring>
-#include <memory>
-
 #include <hal/Ports.h>
 #include <imgui.h>
 #include <mockdata/DIOData.h>
@@ -20,9 +16,13 @@
 #include <mockdata/SimDeviceData.h>
 
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
+static IniSaver<NameInfo> gDIO{"DIO"};
+
 static void LabelSimDevice(const char* name, HAL_SimDeviceHandle simDevice) {
   ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
   ImGui::LabelText(name, "%s", HALSIM_GetSimDeviceName(simDevice));
@@ -71,9 +71,10 @@ static void DisplayDIO() {
   for (int i = 0; i < numDIO; ++i) {
     if (HALSIM_GetDIOInitialized(i)) {
       hasAny = true;
-      char name[32];
+      auto& info = gDIO[i];
+      char name[128];
       if (pwmMap[i] > 0) {
-        std::snprintf(name, sizeof(name), "PWM[%d]", i);
+        info.GetName(name, sizeof(name), "PWM", i);
         if (auto simDevice = HALSIM_GetDIOSimDevice(i)) {
           LabelSimDevice(name, simDevice);
         } else {
@@ -81,7 +82,7 @@ static void DisplayDIO() {
                            HALSIM_GetDigitalPWMDutyCycle(pwmMap[i] - 1));
         }
       } else if (encoderMap[i] > 0) {
-        std::snprintf(name, sizeof(name), " In[%d]", i);
+        info.GetName(name, sizeof(name), " In", i);
         if (auto simDevice = HALSIM_GetEncoderSimDevice(encoderMap[i] - 1)) {
           LabelSimDevice(name, simDevice);
         } else {
@@ -92,7 +93,7 @@ static void DisplayDIO() {
           ImGui::PopStyleColor();
         }
       } else if (dutyCycleMap[i] > 0) {
-        std::snprintf(name, sizeof(name), "PWM[%d]", i);
+        info.GetName(name, sizeof(name), "Dty", i);
         if (auto simDevice =
                 HALSIM_GetDutyCycleSimDevice(dutyCycleMap[i] - 1)) {
           LabelSimDevice(name, simDevice);
@@ -102,7 +103,7 @@ static void DisplayDIO() {
             HALSIM_SetDutyCycleOutput(dutyCycleMap[i] - 1, val);
         }
       } else if (!HALSIM_GetDIOIsInput(i)) {
-        std::snprintf(name, sizeof(name), "Out[%d]", i);
+        info.GetName(name, sizeof(name), "Out", i);
         if (auto simDevice = HALSIM_GetDIOSimDevice(i)) {
           LabelSimDevice(name, simDevice);
         } else {
@@ -110,7 +111,7 @@ static void DisplayDIO() {
                            HALSIM_GetDIOValue(i) ? "1 (high)" : "0 (low)");
         }
       } else {
-        std::snprintf(name, sizeof(name), " In[%d]", i);
+        info.GetName(name, sizeof(name), " In", i);
         if (auto simDevice = HALSIM_GetDIOSimDevice(i)) {
           LabelSimDevice(name, simDevice);
         } else {
@@ -119,6 +120,7 @@ static void DisplayDIO() {
           if (ImGui::Combo(name, &val, options, 2)) HALSIM_SetDIOValue(i, val);
         }
       }
+      info.PopupEditName(i);
     }
   }
   ImGui::PopItemWidth();
@@ -126,6 +128,7 @@ static void DisplayDIO() {
 }
 
 void DIOGui::Initialize() {
+  gDIO.Initialize();
   HALSimGui::AddWindow("DIO", DisplayDIO, ImGuiWindowFlags_AlwaysAutoResize);
   HALSimGui::SetDefaultWindowPos("DIO", 470, 20);
 }

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -22,6 +22,7 @@
 
 #include "ExtraGuiWidgets.h"
 #include "HALSimGui.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
@@ -45,6 +46,7 @@ struct SystemJoystick {
 };
 
 struct RobotJoystick {
+  NameInfo name;
   std::string guid;
   const SystemJoystick* sys = nullptr;
   bool useGamepad = false;
@@ -93,6 +95,8 @@ static void JoystickReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
     int num;
     if (value.getAsInteger(10, num)) return;
     joy->useGamepad = num;
+  } else {
+    joy->name.ReadIni(name, value);
   }
 }
 
@@ -100,11 +104,15 @@ static void JoystickWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
                              ImGuiTextBuffer* out_buf) {
   for (int i = 0; i < HAL_kMaxJoysticks; ++i) {
     auto& joy = gRobotJoysticks[i];
-    if (!joy.sys) continue;
-    const char* guid = glfwGetJoystickGUID(joy.sys - gSystemJoysticks);
-    if (!guid) continue;
-    out_buf->appendf("[Joystick][%d]\nguid=%s\nuseGamepad=%d\n\n", i, guid,
+    if (!joy.name.HasName() && !joy.sys) continue;
+    out_buf->appendf("[Joystick][%d]\nuseGamepad=%d\n", i,
                      joy.useGamepad ? 1 : 0);
+    if (joy.name.HasName()) joy.name.WriteIni(out_buf);
+    if (joy.sys) {
+      const char* guid = glfwGetJoystickGUID(joy.sys - gSystemJoysticks);
+      if (guid) out_buf->appendf("guid=%s\n", guid);
+    }
+    out_buf->append("\n");
   }
 }
 
@@ -426,8 +434,8 @@ static void DisplayJoysticks() {
   ImGui::Columns(HAL_kMaxJoysticks, "Joysticks", false);
   for (int i = 0; i < HAL_kMaxJoysticks; ++i) {
     auto& joy = gRobotJoysticks[i];
-    char label[30];
-    std::snprintf(label, sizeof(label), "Joystick %d", i);
+    char label[128];
+    joy.name.GetName(label, sizeof(label), "Joystick", i);
     if (joy.sys) {
       ImGui::Selectable(label, false);
       if (ImGui::BeginDragDropSource()) {
@@ -455,6 +463,7 @@ static void DisplayJoysticks() {
       }
       ImGui::EndDragDropTarget();
     }
+    joy.name.PopupEditName(i);
     ImGui::NextColumn();
   }
   ImGui::Separator();

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,49 +7,32 @@
 
 #include "EncoderGui.h"
 
-#include <cstdio>
-
 #include <hal/Ports.h>
 #include <imgui.h>
-#include <imgui_internal.h>
 #include <mockdata/EncoderData.h>
 #include <mockdata/SimDeviceData.h>
-#include <wpi/DenseMap.h>
-#include <wpi/StringRef.h>
 
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
-static wpi::DenseMap<int, bool> gEncodersOpen;  // indexed by channel A
-
-// read/write open state to ini file
-static void* EncodersReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                              const char* name) {
-  int num;
-  if (wpi::StringRef{name}.getAsInteger(10, num)) return nullptr;
-  return &gEncodersOpen[num];
-}
-
-static void EncodersReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                             void* entry, const char* lineStr) {
-  bool* element = static_cast<bool*>(entry);
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
-  if (name == "open") {
-    int num;
-    if (value.getAsInteger(10, num)) return;
-    *element = num;
+namespace {
+struct EncoderInfo : public NameInfo, public OpenInfo {
+  bool ReadIni(wpi::StringRef name, wpi::StringRef value) {
+    if (NameInfo::ReadIni(name, value)) return true;
+    if (OpenInfo::ReadIni(name, value)) return true;
+    return false;
   }
-}
+  void WriteIni(ImGuiTextBuffer* out) {
+    NameInfo::WriteIni(out);
+    OpenInfo::WriteIni(out);
+  }
+};
+}  // namespace
 
-static void EncodersWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                             ImGuiTextBuffer* out_buf) {
-  for (auto it : gEncodersOpen)
-    out_buf->appendf("[Encoder][%d]\nopen=%d\n\n", it.first, it.second ? 1 : 0);
-}
+static IniSaver<EncoderInfo> gEncoders{"Encoder"};  // indexed by channel A
 
 static void DisplayEncoders() {
   bool hasAny = false;
@@ -58,54 +41,61 @@ static void DisplayEncoders() {
   for (int i = 0; i < numEncoder; ++i) {
     if (HALSIM_GetEncoderInitialized(i)) {
       hasAny = true;
-      char name[32];
-      int chA = HALSIM_GetEncoderDigitalChannelA(i);
-      int chB = HALSIM_GetEncoderDigitalChannelB(i);
-      std::snprintf(name, sizeof(name), "Encoder[%d,%d]", chA, chB);
       if (auto simDevice = HALSIM_GetEncoderSimDevice(i)) {
         ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
         ImGui::Text("%s", HALSIM_GetSimDeviceName(simDevice));
         ImGui::PopStyleColor();
-      } else if (ImGui::CollapsingHeader(
-                     name,
-                     gEncodersOpen[chA] ? ImGuiTreeNodeFlags_DefaultOpen : 0)) {
-        gEncodersOpen[chA] = true;
-
-        ImGui::PushID(i);
-
-        // distance per pulse
-        double distancePerPulse = HALSIM_GetEncoderDistancePerPulse(i);
-        ImGui::LabelText("Dist/Count", "%.6f", distancePerPulse);
-
-        // count
-        int count = HALSIM_GetEncoderCount(i);
-        if (ImGui::InputInt("Count", &count)) HALSIM_SetEncoderCount(i, count);
-        ImGui::SameLine();
-        if (ImGui::Button("Reset")) HALSIM_SetEncoderCount(i, 0);
-
-        // max period
-        double maxPeriod = HALSIM_GetEncoderMaxPeriod(i);
-        ImGui::LabelText("Max Period", "%.6f", maxPeriod);
-
-        // period
-        double period = HALSIM_GetEncoderPeriod(i);
-        if (ImGui::InputDouble("Period", &period, 0, 0, "%.6g"))
-          HALSIM_SetEncoderPeriod(i, period);
-
-        // reverse direction
-        ImGui::LabelText(
-            "Reverse Direction", "%s",
-            HALSIM_GetEncoderReverseDirection(i) ? "true" : "false");
-
-        // direction
-        static const char* options[] = {"reverse", "forward"};
-        int direction = HALSIM_GetEncoderDirection(i) ? 1 : 0;
-        if (ImGui::Combo("Direction", &direction, options, 2))
-          HALSIM_SetEncoderDirection(i, direction);
-
-        ImGui::PopID();
       } else {
-        gEncodersOpen[chA] = false;
+        int chA = HALSIM_GetEncoderDigitalChannelA(i);
+        int chB = HALSIM_GetEncoderDigitalChannelB(i);
+
+        // build header name
+        auto& info = gEncoders[chA];
+        char name[128];
+        info.GetName(name, sizeof(name), "Encoder", chA, chB);
+
+        // header
+        bool open = ImGui::CollapsingHeader(
+            name, gEncoders[chA].IsOpen() ? ImGuiTreeNodeFlags_DefaultOpen : 0);
+        info.SetOpen(open);
+
+        // context menu to change name
+        info.PopupEditName(chA);
+
+        if (open) {
+          ImGui::PushID(i);
+          // distance per pulse
+          double distancePerPulse = HALSIM_GetEncoderDistancePerPulse(i);
+          ImGui::LabelText("Dist/Count", "%.6f", distancePerPulse);
+
+          // count
+          int count = HALSIM_GetEncoderCount(i);
+          if (ImGui::InputInt("Count", &count))
+            HALSIM_SetEncoderCount(i, count);
+          ImGui::SameLine();
+          if (ImGui::Button("Reset")) HALSIM_SetEncoderCount(i, 0);
+
+          // max period
+          double maxPeriod = HALSIM_GetEncoderMaxPeriod(i);
+          ImGui::LabelText("Max Period", "%.6f", maxPeriod);
+
+          // period
+          double period = HALSIM_GetEncoderPeriod(i);
+          if (ImGui::InputDouble("Period", &period, 0, 0, "%.6g"))
+            HALSIM_SetEncoderPeriod(i, period);
+
+          // reverse direction
+          ImGui::LabelText(
+              "Reverse Direction", "%s",
+              HALSIM_GetEncoderReverseDirection(i) ? "true" : "false");
+
+          // direction
+          static const char* options[] = {"reverse", "forward"};
+          int direction = HALSIM_GetEncoderDirection(i) ? 1 : 0;
+          if (ImGui::Combo("Direction", &direction, options, 2))
+            HALSIM_SetEncoderDirection(i, direction);
+          ImGui::PopID();
+        }
       }
     }
   }
@@ -114,15 +104,7 @@ static void DisplayEncoders() {
 }
 
 void EncoderGui::Initialize() {
-  // hook ini handler to save settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = "Encoder";
-  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
-  iniHandler.ReadOpenFn = EncodersReadOpen;
-  iniHandler.ReadLineFn = EncodersReadLine;
-  iniHandler.WriteAllFn = EncodersWriteAll;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-
+  gEncoders.Initialize();
   HALSimGui::AddWindow("Encoders", DisplayEncoders,
                        ImGuiWindowFlags_AlwaysAutoResize);
   HALSimGui::SetDefaultWindowPos("Encoders", 640, 215);

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

@@ -0,0 +1,649 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Field2D.h"
+
+#include <cmath>
+
+#include <GL/gl3w.h>
+#include <hal/SimDevice.h>
+#include <imgui.h>
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+#include <imgui_internal.h>
+#include <mockdata/SimDeviceData.h>
+#include <units/units.h>
+#include <wpi/Path.h>
+#include <wpi/SmallString.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+#include "GuiUtil.h"
+#include "HALSimGui.h"
+#include "SimDeviceGui.h"
+#include "portable-file-dialogs.h"
+
+using namespace halsimgui;
+
+namespace {
+
+// Per-frame field data (not persistent)
+struct FieldFrameData {
+  // in window coordinates
+  ImVec2 imageMin;
+  ImVec2 imageMax;
+  ImVec2 min;
+  ImVec2 max;
+
+  float scale;  // scaling from field units to screen units
+};
+
+class FieldInfo {
+ public:
+  static constexpr float kDefaultWidth = 15.98f;
+  static constexpr float kDefaultHeight = 8.21f;
+
+  std::unique_ptr<pfd::open_file> m_fileOpener;
+  float m_width = kDefaultWidth;
+  float m_height = kDefaultHeight;
+
+  void Reset();
+  void LoadImage();
+  void LoadJson(const wpi::Twine& jsonfile);
+  FieldFrameData GetFrameData() const;
+  void Draw(ImDrawList* drawList, const ImVec2& windowPos,
+            const FieldFrameData& frameData) const;
+
+  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
+  void WriteIni(ImGuiTextBuffer* out) const;
+
+ private:
+  bool LoadImageImpl(const wpi::Twine& fn);
+
+  std::string m_filename;
+  GLuint m_texture = 0;
+  int m_imageWidth = 0;
+  int m_imageHeight = 0;
+  int m_top = 0;
+  int m_left = 0;
+  int m_bottom = -1;
+  int m_right = -1;
+};
+
+// Per-frame robot data (not persistent)
+struct RobotFrameData {
+  // in window coordinates
+  ImVec2 center;
+  ImVec2 corners[4];
+  ImVec2 arrow[3];
+
+  // scaled width/2 and length/2, in screen units
+  float width2;
+  float length2;
+};
+
+class RobotInfo {
+ public:
+  static constexpr float kDefaultWidth = 0.6858f;
+  static constexpr float kDefaultLength = 0.8204f;
+
+  std::unique_ptr<pfd::open_file> m_fileOpener;
+  float m_width = kDefaultWidth;
+  float m_length = kDefaultLength;
+
+  void Reset();
+  void LoadImage();
+  void UpdateFromSimDevice();
+  void SetPosition(double x, double y);
+  // set and get rotation in radians
+  void SetRotation(double rot);
+  double GetRotation() const {
+    return units::convert<units::degrees, units::radians>(m_rot);
+  }
+  RobotFrameData GetFrameData(const FieldFrameData& ffd) const;
+  void Draw(ImDrawList* drawList, const ImVec2& windowPos,
+            const RobotFrameData& frameData, int hit, float hitRadius) const;
+
+  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
+  void WriteIni(ImGuiTextBuffer* out) const;
+
+ private:
+  bool LoadImageImpl(const wpi::Twine& fn);
+
+  std::string m_filename;
+  GLuint m_texture = 0;
+
+  HAL_SimDeviceHandle m_devHandle = 0;
+  hal::SimDouble m_xHandle;
+  hal::SimDouble m_yHandle;
+  hal::SimDouble m_rotHandle;
+
+  double m_x = 0;
+  double m_y = 0;
+  double m_rot = 0;
+};
+
+}  // namespace
+
+static FieldInfo gField;
+static RobotInfo gRobot;
+static int gDragRobot = 0;
+static ImVec2 gDragInitialOffset;
+static double gDragInitialAngle;
+
+// read/write settings to ini file
+static void* Field2DReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                             const char* name) {
+  if (name == wpi::StringRef{"Field"}) return &gField;
+  if (name == wpi::StringRef{"Robot"}) return &gRobot;
+  return nullptr;
+}
+
+static void Field2DReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                            void* entry, const char* lineStr) {
+  wpi::StringRef line{lineStr};
+  auto [name, value] = line.split('=');
+  name = name.trim();
+  value = value.trim();
+  if (entry == &gField)
+    gField.ReadIni(name, value);
+  else if (entry == &gRobot)
+    gRobot.ReadIni(name, value);
+}
+
+static void Field2DWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                            ImGuiTextBuffer* out_buf) {
+  gField.WriteIni(out_buf);
+  gRobot.WriteIni(out_buf);
+}
+
+void FieldInfo::Reset() {
+  if (m_texture != 0) glDeleteTextures(1, &m_texture);
+  m_texture = 0;
+  m_filename.clear();
+  m_imageWidth = 0;
+  m_imageHeight = 0;
+  m_top = 0;
+  m_left = 0;
+  m_bottom = -1;
+  m_right = -1;
+}
+
+void FieldInfo::LoadImage() {
+  if (m_fileOpener && m_fileOpener->ready(0)) {
+    auto result = m_fileOpener->result();
+    if (!result.empty()) {
+      if (wpi::StringRef(result[0]).endswith(".json")) {
+        LoadJson(result[0]);
+      } else {
+        LoadImageImpl(result[0]);
+        m_top = 0;
+        m_left = 0;
+        m_bottom = -1;
+        m_right = -1;
+      }
+    }
+    m_fileOpener.reset();
+  }
+  if (m_texture == 0 && !m_filename.empty()) {
+    if (!LoadImageImpl(m_filename)) m_filename.clear();
+  }
+}
+
+void FieldInfo::LoadJson(const wpi::Twine& jsonfile) {
+  std::error_code ec;
+  wpi::raw_fd_istream f(jsonfile, ec);
+  if (ec) {
+    wpi::errs() << "GUI: could not open field JSON file\n";
+    return;
+  }
+
+  // parse file
+  wpi::json j;
+  try {
+    j = wpi::json::parse(f);
+  } catch (const wpi::json::parse_error& e) {
+    wpi::errs() << "GUI: JSON: could not parse: " << e.what() << '\n';
+  }
+
+  // top level must be an object
+  if (!j.is_object()) {
+    wpi::errs() << "GUI: JSON: does not contain a top object\n";
+    return;
+  }
+
+  // image filename
+  std::string image;
+  try {
+    image = j.at("field-image").get<std::string>();
+  } catch (const wpi::json::exception& e) {
+    wpi::errs() << "GUI: JSON: could not read field-image: " << e.what()
+                << '\n';
+    return;
+  }
+
+  // corners
+  int top, left, bottom, right;
+  try {
+    top = j.at("field-corners").at("top-left").at(1).get<int>();
+    left = j.at("field-corners").at("top-left").at(0).get<int>();
+    bottom = j.at("field-corners").at("bottom-right").at(1).get<int>();
+    right = j.at("field-corners").at("bottom-right").at(0).get<int>();
+  } catch (const wpi::json::exception& e) {
+    wpi::errs() << "GUI: JSON: could not read field-corners: " << e.what()
+                << '\n';
+    return;
+  }
+
+  // size
+  float width;
+  float height;
+  try {
+    width = j.at("field-size").at(0).get<float>();
+    height = j.at("field-size").at(1).get<float>();
+  } catch (const wpi::json::exception& e) {
+    wpi::errs() << "GUI: JSON: could not read field-size: " << e.what() << '\n';
+    return;
+  }
+
+  // units for size
+  std::string unit;
+  try {
+    unit = j.at("field-unit").get<std::string>();
+  } catch (const wpi::json::exception& e) {
+    wpi::errs() << "GUI: JSON: could not read field-unit: " << e.what() << '\n';
+    return;
+  }
+
+  // convert size units to meters
+  if (unit == "foot" || unit == "feet") {
+    width = units::convert<units::feet, units::meters>(width);
+    height = units::convert<units::feet, units::meters>(height);
+  }
+
+  // the image filename is relative to the json file
+  wpi::SmallString<128> pathname;
+  jsonfile.toVector(pathname);
+  wpi::sys::path::remove_filename(pathname);
+  wpi::sys::path::append(pathname, image);
+
+  // load field image
+  if (!LoadImageImpl(pathname)) return;
+
+  // save to field info
+  m_filename = pathname.str();
+  m_top = top;
+  m_left = left;
+  m_bottom = bottom;
+  m_right = right;
+  m_width = width;
+  m_height = height;
+}
+
+bool FieldInfo::LoadImageImpl(const wpi::Twine& fn) {
+  wpi::outs() << "GUI: loading field image '" << fn << "'\n";
+  GLuint oldTexture = m_texture;
+  if (!LoadTextureFromFile(fn, &m_texture, &m_imageWidth, &m_imageHeight)) {
+    wpi::errs() << "GUI: could not read field image\n";
+    return false;
+  }
+  if (oldTexture != 0) glDeleteTextures(1, &oldTexture);
+  m_filename = fn.str();
+  return true;
+}
+
+FieldFrameData FieldInfo::GetFrameData() const {
+  FieldFrameData ffd;
+
+  // get window content region
+  ffd.imageMin = ImGui::GetWindowContentRegionMin();
+  ffd.imageMax = ImGui::GetWindowContentRegionMax();
+
+  // fit the image into the window
+  if (m_texture != 0 && m_imageHeight != 0 && m_imageWidth != 0)
+    MaxFit(&ffd.imageMin, &ffd.imageMax, m_imageWidth, m_imageHeight);
+
+  ImVec2 min = ffd.imageMin;
+  ImVec2 max = ffd.imageMax;
+
+  // size down the box by the image corners (if any)
+  if (m_bottom > 0 && m_right > 0) {
+    min.x += m_left * (max.x - min.x) / m_imageWidth;
+    min.y += m_top * (max.y - min.y) / m_imageHeight;
+    max.x -= (m_imageWidth - m_right) * (max.x - min.x) / m_imageWidth;
+    max.y -= (m_imageHeight - m_bottom) * (max.y - min.y) / m_imageHeight;
+  }
+
+  // draw the field "active area" as a yellow boundary box
+  MaxFit(&min, &max, m_width, m_height);
+
+  ffd.min = min;
+  ffd.max = max;
+  ffd.scale = (max.x - min.x) / m_width;
+  return ffd;
+}
+
+void FieldInfo::Draw(ImDrawList* drawList, const ImVec2& windowPos,
+                     const FieldFrameData& ffd) const {
+  if (m_texture != 0 && m_imageHeight != 0 && m_imageWidth != 0) {
+    drawList->AddImage(
+        reinterpret_cast<ImTextureID>(static_cast<uintptr_t>(m_texture)),
+        windowPos + ffd.imageMin, windowPos + ffd.imageMax);
+  }
+
+  // draw the field "active area" as a yellow boundary box
+  drawList->AddRect(windowPos + ffd.min, windowPos + ffd.max,
+                    IM_COL32(255, 255, 0, 255));
+}
+
+bool FieldInfo::ReadIni(wpi::StringRef name, wpi::StringRef value) {
+  if (name == "image") {
+    m_filename = value;
+  } else if (name == "top") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_top = num;
+  } else if (name == "left") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_left = num;
+  } else if (name == "bottom") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_bottom = num;
+  } else if (name == "right") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_right = num;
+  } else if (name == "width") {
+    std::sscanf(value.data(), "%f", &m_width);
+  } else if (name == "height") {
+    std::sscanf(value.data(), "%f", &m_height);
+  } else {
+    return false;
+  }
+  return true;
+}
+
+void FieldInfo::WriteIni(ImGuiTextBuffer* out) const {
+  out->appendf(
+      "[Field2D][Field]\nimage=%s\ntop=%d\nleft=%d\nbottom=%d\nright=%d\nwidth="
+      "%f\nheight=%f\n\n",
+      m_filename.c_str(), m_top, m_left, m_bottom, m_right, m_width, m_height);
+}
+
+void RobotInfo::Reset() {
+  if (m_texture != 0) glDeleteTextures(1, &m_texture);
+  m_texture = 0;
+  m_filename.clear();
+}
+
+void RobotInfo::LoadImage() {
+  if (m_fileOpener && m_fileOpener->ready(0)) {
+    auto result = m_fileOpener->result();
+    if (!result.empty()) LoadImageImpl(result[0]);
+    m_fileOpener.reset();
+  }
+  if (m_texture == 0 && !m_filename.empty()) {
+    if (!LoadImageImpl(m_filename)) m_filename.clear();
+  }
+}
+
+bool RobotInfo::LoadImageImpl(const wpi::Twine& fn) {
+  wpi::outs() << "GUI: loading robot image '" << fn << "'\n";
+  GLuint oldTexture = m_texture;
+  if (!LoadTextureFromFile(fn, &m_texture, nullptr, nullptr)) {
+    wpi::errs() << "GUI: could not read robot image\n";
+    return false;
+  }
+  if (oldTexture != 0) glDeleteTextures(1, &oldTexture);
+  m_filename = fn.str();
+  return true;
+}
+
+void RobotInfo::UpdateFromSimDevice() {
+  if (m_devHandle == 0) m_devHandle = HALSIM_GetSimDeviceHandle("Field2D");
+  if (m_devHandle == 0) return;
+
+  if (!m_xHandle) m_xHandle = HALSIM_GetSimValueHandle(m_devHandle, "x");
+  if (m_xHandle) m_x = m_xHandle.Get();
+
+  if (!m_yHandle) m_yHandle = HALSIM_GetSimValueHandle(m_devHandle, "y");
+  if (m_yHandle) m_y = m_yHandle.Get();
+
+  if (!m_rotHandle) m_rotHandle = HALSIM_GetSimValueHandle(m_devHandle, "rot");
+  if (m_rotHandle) m_rot = m_rotHandle.Get();
+}
+
+void RobotInfo::SetPosition(double x, double y) {
+  m_x = x;
+  m_y = y;
+  if (m_xHandle) m_xHandle.Set(x);
+  if (m_yHandle) m_yHandle.Set(y);
+}
+
+void RobotInfo::SetRotation(double rot) {
+  double rotDegrees = units::convert<units::radians, units::degrees>(rot);
+  // force to -180 to +180 range
+  rotDegrees = rotDegrees + std::ceil((-rotDegrees - 180) / 360) * 360;
+  m_rot = rotDegrees;
+  if (m_rotHandle) m_rotHandle.Set(rotDegrees);
+}
+
+RobotFrameData RobotInfo::GetFrameData(const FieldFrameData& ffd) const {
+  RobotFrameData rfd;
+  float width2 = ffd.scale * m_width / 2;
+  float length2 = ffd.scale * m_length / 2;
+
+  // (0,0) origin is bottom left
+  ImVec2 center(ffd.min.x + ffd.scale * m_x, ffd.max.y - ffd.scale * m_y);
+
+  // build rotated points around center
+  double rot = GetRotation();
+  float cos_a = std::cos(-rot);
+  float sin_a = std::sin(-rot);
+
+  rfd.corners[0] = center + ImRotate(ImVec2(-length2, -width2), cos_a, sin_a);
+  rfd.corners[1] = center + ImRotate(ImVec2(length2, -width2), cos_a, sin_a);
+  rfd.corners[2] = center + ImRotate(ImVec2(length2, width2), cos_a, sin_a);
+  rfd.corners[3] = center + ImRotate(ImVec2(-length2, width2), cos_a, sin_a);
+  rfd.arrow[0] =
+      center + ImRotate(ImVec2(-length2 / 2, -width2 / 2), cos_a, sin_a);
+  rfd.arrow[1] = center + ImRotate(ImVec2(length2 / 2, 0), cos_a, sin_a);
+  rfd.arrow[2] =
+      center + ImRotate(ImVec2(-length2 / 2, width2 / 2), cos_a, sin_a);
+
+  rfd.center = center;
+  rfd.width2 = width2;
+  rfd.length2 = length2;
+  return rfd;
+}
+
+void RobotInfo::Draw(ImDrawList* drawList, const ImVec2& windowPos,
+                     const RobotFrameData& rfd, int hit,
+                     float hitRadius) const {
+  if (m_texture != 0) {
+    drawList->AddImageQuad(
+        reinterpret_cast<ImTextureID>(static_cast<uintptr_t>(m_texture)),
+        windowPos + rfd.corners[0], windowPos + rfd.corners[1],
+        windowPos + rfd.corners[2], windowPos + rfd.corners[3]);
+  } else {
+    drawList->AddQuad(windowPos + rfd.corners[0], windowPos + rfd.corners[1],
+                      windowPos + rfd.corners[2], windowPos + rfd.corners[3],
+                      IM_COL32(255, 0, 0, 255), 4.0);
+    drawList->AddTriangle(windowPos + rfd.arrow[0], windowPos + rfd.arrow[1],
+                          windowPos + rfd.arrow[2], IM_COL32(0, 255, 0, 255),
+                          4.0);
+  }
+
+  if (hit > 0) {
+    if (hit == 1) {
+      drawList->AddCircle(windowPos + rfd.center, hitRadius,
+                          IM_COL32(0, 255, 0, 255));
+    } else {
+      drawList->AddCircle(windowPos + rfd.corners[hit - 2], hitRadius,
+                          IM_COL32(0, 255, 0, 255));
+    }
+  }
+}
+
+bool RobotInfo::ReadIni(wpi::StringRef name, wpi::StringRef value) {
+  if (name == "image") {
+    m_filename = value;
+  } else if (name == "width") {
+    std::sscanf(value.data(), "%f", &m_width);
+  } else if (name == "length") {
+    std::sscanf(value.data(), "%f", &m_length);
+  } else {
+    return false;
+  }
+  return true;
+}
+
+void RobotInfo::WriteIni(ImGuiTextBuffer* out) const {
+  out->appendf("[Field2D][Robot]\nimage=%s\nwidth=%f\nlength=%f\n\n",
+               m_filename.c_str(), m_width, m_length);
+}
+
+static void OptionMenuField2D() {
+  if (ImGui::BeginMenu("2D Field View")) {
+    if (ImGui::MenuItem("Choose field image...")) {
+      gField.m_fileOpener = std::make_unique<pfd::open_file>(
+          "Choose field image", "",
+          std::vector<std::string>{"Image File",
+                                   "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
+                                   "*.hdr *.pic *.ppm *.pgm",
+                                   "PathWeaver JSON File", "*.json"});
+    }
+    if (ImGui::MenuItem("Reset field image")) {
+      gField.Reset();
+    }
+    if (ImGui::MenuItem("Choose robot image...")) {
+      gRobot.m_fileOpener = std::make_unique<pfd::open_file>(
+          "Choose robot image", "",
+          std::vector<std::string>{"Image File",
+                                   "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
+                                   "*.hdr *.pic *.ppm *.pgm"});
+    }
+    if (ImGui::MenuItem("Reset robot image")) {
+      gRobot.Reset();
+    }
+    ImGui::EndMenu();
+  }
+}
+
+static void DisplayField2DSettings() {
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
+  ImGui::InputFloat("Field Width", &gField.m_width);
+  ImGui::InputFloat("Field Height", &gField.m_height);
+  // ImGui::InputInt("Field Top", &gField.m_top);
+  // ImGui::InputInt("Field Left", &gField.m_left);
+  // ImGui::InputInt("Field Right", &gField.m_right);
+  // ImGui::InputInt("Field Bottom", &gField.m_bottom);
+  ImGui::InputFloat("Robot Width", &gRobot.m_width);
+  ImGui::InputFloat("Robot Length", &gRobot.m_length);
+  ImGui::PopItemWidth();
+}
+
+static void DisplayField2D() {
+  // load images
+  gField.LoadImage();
+  gRobot.LoadImage();
+
+  // get robot coordinates from SimDevice
+  gRobot.UpdateFromSimDevice();
+
+  FieldFrameData ffd = gField.GetFrameData();
+  RobotFrameData rfd = gRobot.GetFrameData(ffd);
+
+  ImVec2 windowPos = ImGui::GetWindowPos();
+
+  // for dragging to work, there needs to be a button (otherwise the window is
+  // dragged)
+  ImGui::InvisibleButton("field", ImGui::GetContentRegionAvail());
+
+  // allow dragging the robot around
+  ImVec2 cursor = ImGui::GetIO().MousePos - windowPos;
+
+  int hit = 0;
+  float hitRadius = (std::min)(rfd.width2, rfd.length2) / 2;
+  // only allow initiation of dragging when invisible button is hovered; this
+  // prevents the window resize handles from simultaneously activating the drag
+  // functionality
+  if (ImGui::IsItemHovered()) {
+    float hitRadiusSquared = hitRadius * hitRadius;
+    // it's within the hit radius of the center?
+    if (GetDistSquared(cursor, rfd.center) < hitRadiusSquared)
+      hit = 1;
+    else if (GetDistSquared(cursor, rfd.corners[0]) < hitRadiusSquared)
+      hit = 2;
+    else if (GetDistSquared(cursor, rfd.corners[1]) < hitRadiusSquared)
+      hit = 3;
+    else if (GetDistSquared(cursor, rfd.corners[2]) < hitRadiusSquared)
+      hit = 4;
+    else if (GetDistSquared(cursor, rfd.corners[3]) < hitRadiusSquared)
+      hit = 5;
+    if (hit > 0 && ImGui::IsMouseClicked(0)) {
+      if (hit == 1) {
+        gDragRobot = hit;
+        gDragInitialOffset = cursor - rfd.center;
+      } else {
+        gDragRobot = hit;
+        ImVec2 off = cursor - rfd.center;
+        gDragInitialAngle = std::atan2(off.y, off.x) + gRobot.GetRotation();
+      }
+    }
+  }
+
+  if (gDragRobot > 0 && ImGui::IsMouseDown(0)) {
+    if (gDragRobot == 1) {
+      ImVec2 newPos = cursor - gDragInitialOffset;
+      gRobot.SetPosition(
+          (std::clamp(newPos.x, ffd.min.x, ffd.max.x) - ffd.min.x) / ffd.scale,
+          (ffd.max.y - std::clamp(newPos.y, ffd.min.y, ffd.max.y)) / ffd.scale);
+      rfd = gRobot.GetFrameData(ffd);
+    } else {
+      ImVec2 off = cursor - rfd.center;
+      gRobot.SetRotation(gDragInitialAngle - std::atan2(off.y, off.x));
+    }
+    hit = gDragRobot;  // keep it highlighted
+  } else {
+    gDragRobot = 0;
+  }
+
+  // draw
+  auto drawList = ImGui::GetWindowDrawList();
+  gField.Draw(drawList, windowPos, ffd);
+  gRobot.Draw(drawList, windowPos, rfd, hit, hitRadius);
+}
+
+void Field2D::Initialize() {
+  // hook ini handler to save settings
+  ImGuiSettingsHandler iniHandler;
+  iniHandler.TypeName = "Field2D";
+  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
+  iniHandler.ReadOpenFn = Field2DReadOpen;
+  iniHandler.ReadLineFn = Field2DReadLine;
+  iniHandler.WriteAllFn = Field2DWriteAll;
+  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
+
+  HALSimGui::AddOptionMenu(OptionMenuField2D);
+
+  HALSimGui::AddWindow("2D Field Settings", DisplayField2DSettings,
+                       ImGuiWindowFlags_AlwaysAutoResize);
+  HALSimGui::SetWindowVisibility("2D Field Settings", HALSimGui::kHide);
+  HALSimGui::SetDefaultWindowPos("2D Field Settings", 200, 150);
+
+  HALSimGui::AddWindow("2D Field View", DisplayField2D);
+  HALSimGui::SetWindowVisibility("2D Field View", HALSimGui::kHide);
+  HALSimGui::SetDefaultWindowPos("2D Field View", 200, 200);
+  HALSimGui::SetDefaultWindowSize("2D Field View", 400, 200);
+  HALSimGui::SetWindowPadding("2D Field View", 0, 0);
+
+  // SimDeviceGui::Hide("Field2D");
+}

simulation/halsim_gui/src/main/native/cpp/Field2D.h

@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace halsimgui {
+
+class Field2D {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui

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

@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "GuiUtil.h"
+
+#include <stb_image.h>
+
+#include <wpi/SmallString.h>
+
+bool halsimgui::LoadTextureFromFile(const wpi::Twine& filename,
+                                    GLuint* out_texture, int* out_width,
+                                    int* out_height) {
+  wpi::SmallString<128> buf;
+
+  // Load from file
+  int width = 0;
+  int height = 0;
+  unsigned char* data =
+      stbi_load(filename.toNullTerminatedStringRef(buf).data(), &width, &height,
+                nullptr, 4);
+  if (!data) return false;
+
+  // Create a OpenGL texture identifier
+  GLuint texture;
+  glGenTextures(1, &texture);
+  glBindTexture(GL_TEXTURE_2D, texture);
+
+  // Setup filtering parameters for display
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+
+  // Upload pixels into texture
+  glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
+  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA,
+               GL_UNSIGNED_BYTE, data);
+  stbi_image_free(data);
+
+  *out_texture = texture;
+  if (out_width) *out_width = width;
+  if (out_height) *out_height = height;
+
+  return true;
+}
+
+void halsimgui::MaxFit(ImVec2* min, ImVec2* max, float width, float height) {
+  float destWidth = max->x - min->x;
+  float destHeight = max->y - min->y;
+  if (width == 0 || height == 0) return;
+  if (destWidth * height > destHeight * width) {
+    float outputWidth = width * destHeight / height;
+    min->x += (destWidth - outputWidth) / 2;
+    max->x -= (destWidth - outputWidth) / 2;
+  } else {
+    float outputHeight = height * destWidth / width;
+    min->y += (destHeight - outputHeight) / 2;
+    max->y -= (destHeight - outputHeight) / 2;
+  }
+}

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -40,6 +40,8 @@ struct WindowInfo {
   ImGuiCond sizeCond = 0;
   ImVec2 pos;
   ImVec2 size;
+  bool setPadding = false;
+  ImVec2 padding;
 };
 }  // namespace
 
@@ -248,6 +250,14 @@ void HALSimGui::SetDefaultWindowSize(const char* name, float width,
   window.size = ImVec2{width, height};
 }
 
+void HALSimGui::SetWindowPadding(const char* name, float x, float y) {
+  auto it = gWindowMap.find(name);
+  if (it == gWindowMap.end()) return;
+  auto& window = gWindows[it->second];
+  window.setPadding = true;
+  window.padding = ImVec2{x, y};
+}
+
 bool HALSimGui::AreOutputsDisabled() {
   return gDisableOutputsOnDSDisable && !HALSIM_GetDriverStationEnabled();
 }
@@ -301,6 +311,12 @@ bool HALSimGui::Initialize() {
   // Set initial window settings
   glfwWindowHint(GLFW_MAXIMIZED, gWindowMaximized ? GLFW_TRUE : GLFW_FALSE);
 
+  if (gWindowWidth == 0 || gWindowHeight == 0) {
+    gWindowWidth = 1280;
+    gWindowHeight = 720;
+    gWindowLoadedWidthHeight = false;
+  }
+
   float windowScale = 1.0;
   if (!gWindowLoadedWidthHeight) {
     glfwWindowHint(GLFW_SCALE_TO_MONITOR, GLFW_TRUE);
@@ -515,9 +531,12 @@ void HALSimGui::Main(void*) {
           ImGui::SetNextWindowPos(window.pos, window.posCond);
         if (window.sizeCond != 0)
           ImGui::SetNextWindowSize(window.size, window.sizeCond);
+        if (window.setPadding)
+          ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, window.padding);
         if (ImGui::Begin(window.name.c_str(), &window.visible, window.flags))
           window.display();
         ImGui::End();
+        if (window.setPadding) ImGui::PopStyleVar();
       }
     }
 
@@ -591,6 +610,10 @@ void HALSIMGUI_SetDefaultWindowSize(const char* name, float width,
   HALSimGui::SetDefaultWindowSize(name, width, height);
 }
 
+void HALSIMGUI_SetWindowPadding(const char* name, float x, float y) {
+  HALSimGui::SetDefaultWindowSize(name, x, y);
+}
+
 int HALSIMGUI_AreOutputsDisabled(void) {
   return HALSimGui::AreOutputsDisabled();
 }

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

@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "IniSaverInfo.h"
+
+#include <cstdio>
+#include <cstring>
+
+#include <imgui_internal.h>
+
+using namespace halsimgui;
+
+void NameInfo::GetName(char* buf, size_t size, const char* defaultName,
+                       int index) {
+  if (m_name[0] != '\0') {
+    std::snprintf(buf, size, "%s [%d]###Name%d", m_name, index, index);
+  } else {
+    std::snprintf(buf, size, "%s[%d]###Name%d", defaultName, index, index);
+  }
+}
+
+void NameInfo::GetName(char* buf, size_t size, const char* defaultName,
+                       int index, int index2) {
+  if (m_name[0] != '\0') {
+    std::snprintf(buf, size, "%s [%d,%d]###Name%d", m_name, index, index2,
+                  index);
+  } else {
+    std::snprintf(buf, size, "%s[%d,%d]###Name%d", defaultName, index, index2,
+                  index);
+  }
+}
+
+bool NameInfo::ReadIni(wpi::StringRef name, wpi::StringRef value) {
+  if (name != "name") return false;
+  size_t len = (std::min)(value.size(), sizeof(m_name) - 1);
+  std::memcpy(m_name, value.data(), len);
+  m_name[len] = '\0';
+  return true;
+}
+
+void NameInfo::WriteIni(ImGuiTextBuffer* out) {
+  out->appendf("name=%s\n", m_name);
+}
+
+void NameInfo::PushEditNameId(int index) {
+  char id[64];
+  std::snprintf(id, sizeof(id), "Name%d", index);
+  ImGui::PushID(id);
+}
+
+void NameInfo::PopupEditName(int index) {
+  char id[64];
+  std::snprintf(id, sizeof(id), "Name%d", index);
+  if (ImGui::BeginPopupContextItem(id)) {
+    ImGui::Text("Edit name:");
+    if (ImGui::InputText("##edit", m_name, sizeof(m_name),
+                         ImGuiInputTextFlags_EnterReturnsTrue))
+      ImGui::CloseCurrentPopup();
+    if (ImGui::Button("Close")) ImGui::CloseCurrentPopup();
+    ImGui::EndPopup();
+  }
+}
+
+bool OpenInfo::ReadIni(wpi::StringRef name, wpi::StringRef value) {
+  if (name != "open") return false;
+  int num;
+  if (value.getAsInteger(10, num)) return true;
+  m_open = num;
+  return true;
+}
+
+void OpenInfo::WriteIni(ImGuiTextBuffer* out) {
+  out->appendf("open=%d\n", m_open ? 1 : 0);
+}

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,9 @@
 
 #include "PDPGui.h"
 
+#include <algorithm>
 #include <cstdio>
+#include <cstring>
 #include <memory>
 
 #include <hal/Ports.h>
@@ -15,64 +17,82 @@
 #include <mockdata/PDPData.h>
 
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
+static IniSaver<NameInfo> gChannels{"PDP"};
+
 static void DisplayPDP() {
   bool hasAny = false;
   static int numPDP = HAL_GetNumPDPModules();
   static int numChannels = HAL_GetNumPDPChannels();
   static auto channelCurrents = std::make_unique<double[]>(numChannels);
-  ImGui::PushItemWidth(ImGui::GetFontSize() * 13);
   for (int i = 0; i < numPDP; ++i) {
     if (HALSIM_GetPDPInitialized(i)) {
       hasAny = true;
 
-      char name[32];
+      char name[128];
       std::snprintf(name, sizeof(name), "PDP[%d]", i);
       if (ImGui::CollapsingHeader(name, ImGuiTreeNodeFlags_DefaultOpen)) {
         ImGui::PushID(i);
 
         // temperature
         double temp = HALSIM_GetPDPTemperature(i);
-        if (ImGui::InputDouble("Temp", &temp))
+        ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+        if (ImGui::InputDouble("Temp", &temp, 0, 0, "%.3f"))
           HALSIM_SetPDPTemperature(i, temp);
 
         // voltage
         double volts = HALSIM_GetPDPVoltage(i);
-        if (ImGui::InputDouble("Voltage", &volts))
+        ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+        if (ImGui::InputDouble("Voltage", &volts, 0, 0, "%.3f"))
           HALSIM_SetPDPVoltage(i, volts);
 
         // channel currents; show as two columns laid out like PDP
         HALSIM_GetPDPAllCurrents(i, channelCurrents.get());
         ImGui::Text("Channel Current (A)");
         ImGui::Columns(2, "channels", false);
+        float maxWidth = ImGui::GetFontSize() * 13;
         for (int left = 0, right = numChannels - 1; left < right;
              ++left, --right) {
           double val;
 
-          std::snprintf(name, sizeof(name), "[%d]", left);
+          auto& leftInfo = gChannels[i * numChannels + left];
+          leftInfo.GetName(name, sizeof(name), "", left);
           val = channelCurrents[left];
-          if (ImGui::InputDouble(name, &val))
+          ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+          if (ImGui::InputDouble(name, &val, 0, 0, "%.3f"))
             HALSIM_SetPDPCurrent(i, left, val);
+          float leftWidth = ImGui::GetItemRectSize().x;
+          leftInfo.PopupEditName(left);
           ImGui::NextColumn();
 
-          std::snprintf(name, sizeof(name), "[%d]", right);
+          auto& rightInfo = gChannels[i * numChannels + right];
+          rightInfo.GetName(name, sizeof(name), "", right);
           val = channelCurrents[right];
-          if (ImGui::InputDouble(name, &val))
+          ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+          if (ImGui::InputDouble(name, &val, 0, 0, "%.3f"))
             HALSIM_SetPDPCurrent(i, right, val);
+          float rightWidth = ImGui::GetItemRectSize().x;
+          rightInfo.PopupEditName(right);
           ImGui::NextColumn();
+
+          float width = (std::max)(leftWidth, rightWidth) * 2;
+          if (width > maxWidth) maxWidth = width;
         }
         ImGui::Columns(1);
+        ImGui::Dummy(ImVec2(maxWidth, 0));
         ImGui::PopID();
       }
     }
   }
-  ImGui::PopItemWidth();
   if (!hasAny) ImGui::Text("No PDPs");
 }
 
 void PDPGui::Initialize() {
+  gChannels.Initialize();
   HALSimGui::AddWindow("PDP", DisplayPDP, ImGuiWindowFlags_AlwaysAutoResize);
   // hide it by default
   HALSimGui::SetWindowVisibility("PDP", HALSimGui::kHide);

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,9 +17,13 @@
 #include <mockdata/PWMData.h>
 
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
+static IniSaver<NameInfo> gPWM{"PWM"};
+
 static void DisplayPWMs() {
   bool hasOutputs = false;
   static const int numPWM = HAL_GetNumPWMChannels();
@@ -45,6 +49,7 @@ static void DisplayPWMs() {
   //};
   // static std::vector<std::unique_ptr<History>> history;
   bool first = true;
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
   for (int i = 0; i < numPWM; ++i) {
     if (HALSIM_GetPWMInitialized(i)) {
       hasOutputs = true;
@@ -54,14 +59,16 @@ static void DisplayPWMs() {
       else
         first = false;
 
-      char name[32];
-      std::snprintf(name, sizeof(name), "PWM[%d]", i);
+      char name[128];
+      auto& info = gPWM[i];
+      info.GetName(name, sizeof(name), "PWM", i);
       if (ledMap[i] > 0) {
-        ImGui::Text("%s: LED[%d]", name, ledMap[i] - 1);
+        ImGui::LabelText(name, "LED[%d]", ledMap[i] - 1);
       } else {
         float val = HALSimGui::AreOutputsDisabled() ? 0 : HALSIM_GetPWMSpeed(i);
-        ImGui::Value(name, val, "%0.3f");
+        ImGui::LabelText(name, "%0.3f", val);
       }
+      info.PopupEditName(i);
 
       // lazily build history storage
       // if (static_cast<unsigned int>(i) > history.size())
@@ -74,10 +81,12 @@ static void DisplayPWMs() {
       // );
     }
   }
+  ImGui::PopItemWidth();
   if (!hasOutputs) ImGui::Text("No PWM outputs");
 }
 
 void PWMGui::Initialize() {
+  gPWM.Initialize();
   HALSimGui::AddWindow("PWM Outputs", DisplayPWMs,
                        ImGuiWindowFlags_AlwaysAutoResize);
   HALSimGui::SetDefaultWindowPos("PWM Outputs", 910, 20);

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,9 +16,13 @@
 
 #include "ExtraGuiWidgets.h"
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
+static IniSaver<NameInfo> gRelays{"Relay"};
+
 static void DisplayRelays() {
   bool hasOutputs = false;
   bool first = true;
@@ -42,7 +46,14 @@ static void DisplayRelays() {
         forward = HALSIM_GetRelayForward(i);
       }
 
-      ImGui::Text("Relay[%d]", i);
+      auto& info = gRelays[i];
+      info.PushEditNameId(i);
+      if (info.HasName())
+        ImGui::Text("%s [%d]", info.GetName(), i);
+      else
+        ImGui::Text("Relay[%d]", i);
+      ImGui::PopID();
+      info.PopupEditName(i);
       ImGui::SameLine();
 
       // show forward and reverse as LED indicators
@@ -58,6 +69,7 @@ static void DisplayRelays() {
 }
 
 void RelayGui::Initialize() {
+  gRelays.Initialize();
   HALSimGui::AddWindow("Relays", DisplayRelays,
                        ImGuiWindowFlags_AlwaysAutoResize);
   HALSimGui::SetDefaultWindowPos("Relays", 180, 20);

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,51 +13,22 @@
 
 #include <hal/SimDevice.h>
 #include <imgui.h>
-#include <imgui_internal.h>
 #include <mockdata/SimDeviceData.h>
-#include <wpi/StringMap.h>
 
 #include "HALSimGui.h"
+#include "IniSaverInfo.h"
+#include "IniSaverString.h"
 
 using namespace halsimgui;
 
 namespace {
-struct ElementInfo {
-  bool open = false;
-  bool visible = true;
+struct ElementInfo : public OpenInfo {
+  bool visible = true;  // not saved
 };
 }  // namespace
 
 static std::vector<std::function<void()>> gDeviceExecutors;
-static wpi::StringMap<ElementInfo> gElements;
-
-// read/write open state to ini file
-static void* DevicesReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                             const char* name) {
-  return &gElements[name];
-}
-
-static void DevicesReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                            void* entry, const char* lineStr) {
-  ElementInfo* element = static_cast<ElementInfo*>(entry);
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
-  if (name == "open") {
-    int num;
-    if (value.getAsInteger(10, num)) return;
-    element->open = num;
-  }
-}
-
-static void DevicesWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                            ImGuiTextBuffer* out_buf) {
-  for (auto&& entry : gElements) {
-    out_buf->appendf("[Device][%s]\nopen=%d\n\n", entry.getKey().data(),
-                     entry.getValue().open ? 1 : 0);
-  }
-}
+static IniSaverString<ElementInfo> gElements{"Device"};
 
 void SimDeviceGui::Hide(const char* name) { gElements[name].visible = false; }
 
@@ -70,12 +41,13 @@ bool SimDeviceGui::StartDevice(const char* label, ImGuiTreeNodeFlags flags) {
   if (!element.visible) return false;
 
   if (ImGui::CollapsingHeader(
-          label, flags | (element.open ? ImGuiTreeNodeFlags_DefaultOpen : 0))) {
+          label,
+          flags | (element.IsOpen() ? ImGuiTreeNodeFlags_DefaultOpen : 0))) {
     ImGui::PushID(label);
-    element.open = true;
+    element.SetOpen(true);
     return true;
   }
-  element.open = false;
+  element.SetOpen(false);
   return false;
 }
 
@@ -201,15 +173,7 @@ static void DisplayDeviceTree() {
 }
 
 void SimDeviceGui::Initialize() {
-  // hook ini handler to save device settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = "Device";
-  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
-  iniHandler.ReadOpenFn = DevicesReadOpen;
-  iniHandler.ReadLineFn = DevicesReadLine;
-  iniHandler.WriteAllFn = DevicesWriteAll;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-
+  gElements.Initialize();
   HALSimGui::AddWindow("Other Devices", DisplayDeviceTree);
   HALSimGui::SetDefaultWindowPos("Other Devices", 1025, 20);
   HALSimGui::SetDefaultWindowSize("Other Devices", 250, 695);

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,9 +17,14 @@
 
 #include "ExtraGuiWidgets.h"
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
+static IniSaver<OpenInfo> gPCMs{"PCM"};
+static IniSaver<NameInfo> gSolenoids{"Solenoid"};
+
 static void DisplaySolenoids() {
   bool hasOutputs = false;
   static const int numPCM = HAL_GetNumPCMModules();
@@ -43,18 +48,40 @@ static void DisplaySolenoids() {
     if (!anyInit) continue;
     hasOutputs = true;
 
-    ImGui::Text("PCM[%d]", i);
+    char name[128];
+    std::snprintf(name, sizeof(name), "PCM[%d]", i);
+    auto& pcmInfo = gPCMs[i];
+    bool open = ImGui::CollapsingHeader(
+        name, pcmInfo.IsOpen() ? ImGuiTreeNodeFlags_DefaultOpen : 0);
+    pcmInfo.SetOpen(open);
+    ImGui::SetItemAllowOverlap();
     ImGui::SameLine();
 
     // show channels as LED indicators
     static const ImU32 colors[] = {IM_COL32(255, 255, 102, 255),
                                    IM_COL32(128, 128, 128, 255)};
     DrawLEDs(channels.data(), channels.size(), channels.size(), colors);
+
+    if (open) {
+      ImGui::PushID(i);
+      ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
+      for (int j = 0; j < numChannels; ++j) {
+        if (channels[j] == -2) continue;
+        auto& info = gSolenoids[i * numChannels + j];
+        info.GetName(name, sizeof(name), "Solenoid", j);
+        ImGui::LabelText(name, "%s", channels[j] == 1 ? "On" : "Off");
+        info.PopupEditName(j);
+      }
+      ImGui::PopItemWidth();
+      ImGui::PopID();
+    }
   }
   if (!hasOutputs) ImGui::Text("No solenoids");
 }
 
 void SolenoidGui::Initialize() {
+  gPCMs.Initialize();
+  gSolenoids.Initialize();
   HALSimGui::AddWindow("Solenoids", DisplaySolenoids,
                        ImGuiWindowFlags_AlwaysAutoResize);
   HALSimGui::SetDefaultWindowPos("Solenoids", 290, 20);

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,6 +17,7 @@
 #include "DIOGui.h"
 #include "DriverStationGui.h"
 #include "EncoderGui.h"
+#include "Field2D.h"
 #include "HALSimGui.h"
 #include "PDPGui.h"
 #include "PWMGui.h"
@@ -42,6 +43,7 @@ __declspec(dllexport)
   HALSimGui::Add(DriverStationGui::Initialize);
   HALSimGui::Add(DIOGui::Initialize);
   HALSimGui::Add(EncoderGui::Initialize);
+  HALSimGui::Add(Field2D::Initialize);
   HALSimGui::Add(PDPGui::Initialize);
   HALSimGui::Add(PWMGui::Initialize);
   HALSimGui::Add(RelayGui::Initialize);

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

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <GL/gl3w.h>
+#include <imgui.h>
+#include <wpi/Twine.h>
+
+namespace halsimgui {
+
+bool LoadTextureFromFile(const wpi::Twine& filename, GLuint* out_texture,
+                         int* out_width, int* out_height);
+
+// get distance^2 between two ImVec's
+inline float GetDistSquared(const ImVec2& a, const ImVec2& b) {
+  float deltaX = b.x - a.x;
+  float deltaY = b.y - a.y;
+  return deltaX * deltaX + deltaY * deltaY;
+}
+
+// maximize fit while preserving aspect ratio
+void MaxFit(ImVec2* min, ImVec2* max, float width, float height);
+
+}  // namespace halsimgui

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -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);
+void HALSIMGUI_SetWindowPadding(const char* name, float x, float y);
 int HALSIMGUI_AreOutputsDisabled(void);
 
 }  // extern "C"
@@ -127,6 +128,14 @@ class HALSimGui {
    */
   static void SetDefaultWindowSize(const char* name, float width, float height);
 
+  /**
+   * Sets internal padding of window added with AddWindow().
+   * @param name window name (same as name passed to AddWindow())
+   * @param x horizontal padding
+   * @param y vertical padding
+   */
+  static void SetWindowPadding(const char* name, float x, float y);
+
   /**
    * Returns true if outputs are disabled.
    *

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

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <wpi/DenseMap.h>
+
+namespace halsimgui {
+
+template <typename Info>
+class IniSaver {
+ public:
+  explicit IniSaver(const char* typeName) : m_typeName(typeName) {}
+  void Initialize();
+
+  // pass through useful functions to map
+  Info& operator[](int index) { return m_map[index]; }
+
+  auto begin() { return m_map.begin(); }
+  auto end() { return m_map.end(); }
+  auto find(int index) { return m_map.find(index); }
+
+  auto begin() const { return m_map.begin(); }
+  auto end() const { return m_map.end(); }
+  auto find(int index) const { return m_map.find(index); }
+
+ private:
+  static void* ReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                        const char* name);
+  static void ReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                       void* entry, const char* lineStr);
+  static void WriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                       ImGuiTextBuffer* out_buf);
+
+  const char* m_typeName;
+  wpi::DenseMap<int, Info> m_map;
+};
+
+}  // namespace halsimgui
+
+#include "IniSaver.inl"

simulation/halsim_gui/src/main/native/include/IniSaver.inl

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace halsimgui {
+
+template <typename Info>
+void IniSaver<Info>::Initialize() {
+  // hook ini handler to save settings
+  ImGuiSettingsHandler iniHandler;
+  iniHandler.TypeName = m_typeName;
+  iniHandler.TypeHash = ImHashStr(m_typeName);
+  iniHandler.ReadOpenFn = ReadOpen;
+  iniHandler.ReadLineFn = ReadLine;
+  iniHandler.WriteAllFn = WriteAll;
+  iniHandler.UserData = this;
+  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
+}
+
+template <typename Info>
+void* IniSaver<Info>::ReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                               const char* name) {
+  auto self = static_cast<IniSaver*>(handler->UserData);
+  int num;
+  if (wpi::StringRef{name}.getAsInteger(10, num)) return nullptr;
+  return &self->m_map[num];
+}
+
+template <typename Info>
+void IniSaver<Info>::ReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                              void* entry, const char* lineStr) {
+  auto element = static_cast<Info*>(entry);
+  wpi::StringRef line{lineStr};
+  auto [name, value] = line.split('=');
+  name = name.trim();
+  value = value.trim();
+  element->ReadIni(name, value);
+}
+
+template <typename Info>
+void IniSaver<Info>::WriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                              ImGuiTextBuffer* out_buf) {
+  auto self = static_cast<IniSaver*>(handler->UserData);
+  for (auto&& it : self->m_map) {
+    out_buf->appendf("[%s][%d]\n", self->m_typeName, it.first);
+    it.second.WriteIni(out_buf);
+    out_buf->append("\n");
+  }
+}
+
+}  // namespace halsimgui

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

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <imgui.h>
+#include <wpi/StringRef.h>
+
+namespace halsimgui {
+
+class NameInfo {
+ public:
+  NameInfo() { m_name[0] = '\0'; }
+
+  bool HasName() const { return m_name[0] != '\0'; }
+  const char* GetName() const { return m_name; }
+  void GetName(char* buf, size_t size, const char* defaultName, int index);
+  void GetName(char* buf, size_t size, const char* defaultName, int index,
+               int index2);
+  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
+  void WriteIni(ImGuiTextBuffer* out);
+  void PushEditNameId(int index);
+  void PopupEditName(int index);
+
+ private:
+  char m_name[64];
+};
+
+class OpenInfo {
+ public:
+  OpenInfo() = default;
+  explicit OpenInfo(bool open) : m_open(open) {}
+
+  bool IsOpen() const { return m_open; }
+  void SetOpen(bool open) { m_open = open; }
+  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
+  void WriteIni(ImGuiTextBuffer* out);
+
+ private:
+  bool m_open = false;
+};
+
+}  // namespace halsimgui

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

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <wpi/StringMap.h>
+#include <wpi/StringRef.h>
+
+namespace halsimgui {
+
+template <typename Info>
+class IniSaverString {
+ public:
+  explicit IniSaverString(const char* typeName) : m_typeName(typeName) {}
+  void Initialize();
+
+  // pass through useful functions to map
+  Info& operator[](wpi::StringRef key) { return m_map[key]; }
+
+  auto begin() { return m_map.begin(); }
+  auto end() { return m_map.end(); }
+  auto find(wpi::StringRef key) { return m_map.find(key); }
+
+  auto begin() const { return m_map.begin(); }
+  auto end() const { return m_map.end(); }
+  auto find(wpi::StringRef key) const { return m_map.find(key); }
+
+ private:
+  static void* ReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                        const char* name);
+  static void ReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                       void* entry, const char* lineStr);
+  static void WriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                       ImGuiTextBuffer* out_buf);
+
+  const char* m_typeName;
+  wpi::StringMap<Info> m_map;
+};
+
+}  // namespace halsimgui
+
+#include "IniSaverString.inl"

simulation/halsim_gui/src/main/native/include/IniSaverString.inl

@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace halsimgui {
+
+template <typename Info>
+void IniSaverString<Info>::Initialize() {
+  // hook ini handler to save settings
+  ImGuiSettingsHandler iniHandler;
+  iniHandler.TypeName = m_typeName;
+  iniHandler.TypeHash = ImHashStr(m_typeName);
+  iniHandler.ReadOpenFn = ReadOpen;
+  iniHandler.ReadLineFn = ReadLine;
+  iniHandler.WriteAllFn = WriteAll;
+  iniHandler.UserData = this;
+  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
+}
+
+template <typename Info>
+void* IniSaverString<Info>::ReadOpen(ImGuiContext* ctx,
+                                     ImGuiSettingsHandler* handler,
+                                     const char* name) {
+  auto self = static_cast<IniSaverString*>(handler->UserData);
+  return &self->m_map[name];
+}
+
+template <typename Info>
+void IniSaverString<Info>::ReadLine(ImGuiContext* ctx,
+                                    ImGuiSettingsHandler* handler, void* entry,
+                                    const char* lineStr) {
+  auto element = static_cast<Info*>(entry);
+  wpi::StringRef line{lineStr};
+  auto [name, value] = line.split('=');
+  name = name.trim();
+  value = value.trim();
+  element->ReadIni(name, value);
+}
+
+template <typename Info>
+void IniSaverString<Info>::WriteAll(ImGuiContext* ctx,
+                                    ImGuiSettingsHandler* handler,
+                                    ImGuiTextBuffer* out_buf) {
+  auto self = static_cast<IniSaverString*>(handler->UserData);
+  for (auto&& it : self->m_map) {
+    out_buf->appendf("[%s][%s]\n", self->m_typeName, it.getKey().data());
+    it.second.WriteIni(out_buf);
+    out_buf->append("\n");
+  }
+}
+
+}  // namespace halsimgui

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -24,6 +24,7 @@ import edu.wpi.first.hal.HAL;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
@@ -35,7 +36,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
  * Subsystem#periodic()} methods to be called and for their default commands to be scheduled.
  */
 @SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods", "PMD.TooManyFields"})
-public final class CommandScheduler implements Sendable {
+public final class CommandScheduler implements Sendable, AutoCloseable {
   /**
    * The Singleton Instance.
    */
@@ -70,11 +71,6 @@ public final class CommandScheduler implements Sendable {
 
   private boolean m_disabled;
 
-  //NetworkTable entries for use in Sendable impl
-  private NetworkTableEntry m_namesEntry;
-  private NetworkTableEntry m_idsEntry;
-  private NetworkTableEntry m_cancelEntry;
-
   //Lists of user-supplied actions to be executed on scheduling events for every command.
   private final List<Consumer<Command>> m_initActions = new ArrayList<>();
   private final List<Consumer<Command>> m_executeActions = new ArrayList<>();
@@ -91,6 +87,20 @@ public final class CommandScheduler implements Sendable {
   CommandScheduler() {
     HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand2_Scheduler);
     SendableRegistry.addLW(this, "Scheduler");
+    LiveWindow.setEnabledListener(() -> {
+      disable();
+      cancelAll();
+    });
+    LiveWindow.setDisabledListener(() -> {
+      enable();
+    });
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    LiveWindow.setEnabledListener(null);
+    LiveWindow.setDisabledListener(null);
   }
 
   /**
@@ -375,7 +385,7 @@ public final class CommandScheduler implements Sendable {
    * Cancels all commands that are currently scheduled.
    */
   public void cancelAll() {
-    for (Command command : m_scheduledCommands.keySet()) {
+    for (Command command : m_scheduledCommands.keySet().toArray(new Command[0])) {
       cancel(command);
     }
   }
@@ -473,12 +483,12 @@ public final class CommandScheduler implements Sendable {
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Scheduler");
-    m_namesEntry = builder.getEntry("Names");
-    m_idsEntry = builder.getEntry("Ids");
-    m_cancelEntry = builder.getEntry("Cancel");
+    final NetworkTableEntry namesEntry = builder.getEntry("Names");
+    final NetworkTableEntry idsEntry = builder.getEntry("Ids");
+    final NetworkTableEntry cancelEntry = builder.getEntry("Cancel");
     builder.setUpdateTable(() -> {
 
-      if (m_namesEntry == null || m_idsEntry == null || m_cancelEntry == null) {
+      if (namesEntry == null || idsEntry == null || cancelEntry == null) {
         return;
       }
 
@@ -489,21 +499,21 @@ public final class CommandScheduler implements Sendable {
         ids.put((double) command.hashCode(), command);
       }
 
-      double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]);
+      double[] toCancel = cancelEntry.getDoubleArray(new double[0]);
       if (toCancel.length > 0) {
         for (double hash : toCancel) {
           cancel(ids.get(hash));
           ids.remove(hash);
         }
-        m_cancelEntry.setDoubleArray(new double[0]);
+        cancelEntry.setDoubleArray(new double[0]);
       }
 
       List<String> names = new ArrayList<>();
 
       ids.values().forEach(command -> names.add(command.getName()));
 
-      m_namesEntry.setStringArray(names.toArray(new String[0]));
-      m_idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
+      namesEntry.setStringArray(names.toArray(new String[0]));
+      idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
     });
   }
 }

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -348,6 +348,6 @@ public class MecanumControllerCommand extends CommandBase {
 
   @Override
   public boolean isFinished() {
-    return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+    return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
   }
 }

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -113,7 +113,7 @@ public class ProfiledPIDCommand extends CommandBase {
 
   @Override
   public void initialize() {
-    m_controller.reset();
+    m_controller.reset(m_measurement.getAsDouble());
   }
 
   @Override

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -93,7 +93,7 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
    */
   public void enable() {
     m_enabled = true;
-    m_controller.reset();
+    m_controller.reset(getMeasurement());
   }
 
   /**

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -201,6 +201,6 @@ public class RamseteCommand extends CommandBase {
 
   @Override
   public boolean isFinished() {
-    return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+    return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
   }
 }

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -153,6 +153,6 @@ public class SwerveControllerCommand extends CommandBase {
 
   @Override
   public boolean isFinished() {
-    return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+    return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
   }
 }

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -59,6 +59,6 @@ public class TrapezoidProfileCommand extends CommandBase {
 
   @Override
   public boolean isFinished() {
-    return m_timer.hasPeriodPassed(m_profile.totalTime());
+    return m_timer.hasElapsed(m_profile.totalTime());
   }
 }

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -41,7 +41,7 @@ public class WaitCommand extends CommandBase {
 
   @Override
   public boolean isFinished() {
-    return m_timer.hasPeriodPassed(m_duration);
+    return m_timer.hasElapsed(m_duration);
   }
 
   @Override

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -23,7 +23,7 @@ import edu.wpi.first.wpilibj2.command.Subsystem;
  * wrapper around Trigger with the method names renamed to fit the Button object use.
  */
 @SuppressWarnings("PMD.TooManyMethods")
-public abstract class Button extends Trigger {
+public class Button extends Trigger {
   /**
    * Default constructor; creates a button that is never pressed (unless {@link Button#get()} is
    * overridden).

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

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

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

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

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,12 +9,14 @@
 
 #include <frc/RobotState.h>
 #include <frc/WPIErrors.h>
+#include <frc/livewindow/LiveWindow.h>
 #include <frc/smartdashboard/SendableBuilder.h>
 #include <frc/smartdashboard/SendableRegistry.h>
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <networktables/NetworkTableEntry.h>
 #include <wpi/DenseMap.h>
+#include <wpi/SmallVector.h>
 
 #include "frc2/command/CommandGroupBase.h"
 #include "frc2/command/CommandState.h"
@@ -42,11 +44,6 @@ class CommandScheduler::Impl {
 
   bool disabled{false};
 
-  // NetworkTable entries for use in Sendable impl
-  nt::NetworkTableEntry namesEntry;
-  nt::NetworkTableEntry idsEntry;
-  nt::NetworkTableEntry cancelEntry;
-
   // Lists of user-supplied actions to be executed on scheduling events for
   // every command.
   wpi::SmallVector<Action, 4> initActions;
@@ -71,9 +68,20 @@ CommandScheduler::CommandScheduler() : m_impl(new Impl) {
   HAL_Report(HALUsageReporting::kResourceType_Command,
              HALUsageReporting::kCommand2_Scheduler);
   frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = [this] {
+    this->Disable();
+    this->CancelAll();
+  };
+  scheduler->disabled = [this] { this->Enable(); };
 }
 
-CommandScheduler::~CommandScheduler() {}
+CommandScheduler::~CommandScheduler() {
+  frc::SendableRegistry::GetInstance().Remove(this);
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = nullptr;
+  scheduler->disabled = nullptr;
+}
 
 CommandScheduler& CommandScheduler::GetInstance() {
   static CommandScheduler scheduler;
@@ -248,6 +256,12 @@ void CommandScheduler::RegisterSubsystem(
   }
 }
 
+void CommandScheduler::RegisterSubsystem(wpi::ArrayRef<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    RegisterSubsystem(subsystem);
+  }
+}
+
 void CommandScheduler::UnregisterSubsystem(
     std::initializer_list<Subsystem*> subsystems) {
   for (auto* subsystem : subsystems) {
@@ -255,6 +269,13 @@ void CommandScheduler::UnregisterSubsystem(
   }
 }
 
+void CommandScheduler::UnregisterSubsystem(
+    wpi::ArrayRef<Subsystem*> subsystems) {
+  for (auto* subsystem : subsystems) {
+    UnregisterSubsystem(subsystem);
+  }
+}
+
 Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
   auto&& find = m_impl->subsystems.find(subsystem);
   if (find != m_impl->subsystems.end()) {
@@ -297,9 +318,11 @@ void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
 }
 
 void CommandScheduler::CancelAll() {
+  wpi::SmallVector<Command*, 16> commands;
   for (auto&& command : m_impl->scheduledCommands) {
-    Cancel(command.first);
+    commands.emplace_back(command.first);
   }
+  Cancel(commands);
 }
 
 double CommandScheduler::TimeSinceScheduled(const Command* command) const {
@@ -366,14 +389,14 @@ void CommandScheduler::OnCommandFinish(Action action) {
 
 void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Scheduler");
-  m_impl->namesEntry = builder.GetEntry("Names");
-  m_impl->idsEntry = builder.GetEntry("Ids");
-  m_impl->cancelEntry = builder.GetEntry("Cancel");
+  auto namesEntry = builder.GetEntry("Names");
+  auto idsEntry = builder.GetEntry("Ids");
+  auto cancelEntry = builder.GetEntry("Cancel");
 
-  builder.SetUpdateTable([this] {
+  builder.SetUpdateTable([=] {
     double tmp[1];
     tmp[0] = 0;
-    auto toCancel = m_impl->cancelEntry.GetDoubleArray(tmp);
+    auto toCancel = cancelEntry.GetDoubleArray(tmp);
     for (auto cancel : toCancel) {
       uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
       Command* command = reinterpret_cast<Command*>(ptrTmp);
@@ -381,7 +404,8 @@ void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
           m_impl->scheduledCommands.end()) {
         Cancel(command);
       }
-      m_impl->cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
+      nt::NetworkTableEntry(cancelEntry)
+          .SetDoubleArray(wpi::ArrayRef<double>{});
     }
 
     wpi::SmallVector<std::string, 8> names;
@@ -391,8 +415,8 @@ void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
       uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
       ids.emplace_back(static_cast<double>(ptrTmp));
     }
-    m_impl->namesEntry.SetStringArray(names);
-    m_impl->idsEntry.SetDoubleArray(ids);
+    nt::NetworkTableEntry(namesEntry).SetStringArray(names);
+    nt::NetworkTableEntry(idsEntry).SetDoubleArray(ids);
   });
 }
 

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

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

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

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

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

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

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

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

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

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

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,6 +15,7 @@ ParallelRaceGroup::ParallelRaceGroup(
 }
 
 void ParallelRaceGroup::Initialize() {
+  m_finished = false;
   for (auto& commandRunning : m_commands) {
     commandRunning->Initialize();
   }

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

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

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

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

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

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

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,6 +21,6 @@ void WaitCommand::Initialize() {
 
 void WaitCommand::End(bool interrupted) { m_timer.Stop(); }
 
-bool WaitCommand::IsFinished() { return m_timer.HasPeriodPassed(m_duration); }
+bool WaitCommand::IsFinished() { return m_timer.HasElapsed(m_duration); }
 
 bool WaitCommand::RunsWhenDisabled() const { return true; }

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

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

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,8 +15,8 @@ Trigger::Trigger(const Trigger& other) : m_isActive(other.m_isActive) {}
 
 Trigger Trigger::WhenActive(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (!pressedLast && pressed) {
           command->Schedule(interruptible);
@@ -30,13 +30,19 @@ Trigger Trigger::WhenActive(Command* command, bool interruptible) {
 
 Trigger Trigger::WhenActive(std::function<void()> toRun,
                             std::initializer_list<Subsystem*> requirements) {
+  return WhenActive(std::move(toRun), wpi::makeArrayRef(requirements.begin(),
+                                                        requirements.end()));
+}
+
+Trigger Trigger::WhenActive(std::function<void()> toRun,
+                            wpi::ArrayRef<Subsystem*> requirements) {
   return WhenActive(InstantCommand(std::move(toRun), requirements));
 }
 
 Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (pressed) {
           command->Schedule(interruptible);
@@ -52,13 +58,20 @@ Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
 Trigger Trigger::WhileActiveContinous(
     std::function<void()> toRun,
     std::initializer_list<Subsystem*> requirements) {
+  return WhileActiveContinous(
+      std::move(toRun),
+      wpi::makeArrayRef(requirements.begin(), requirements.end()));
+}
+
+Trigger Trigger::WhileActiveContinous(std::function<void()> toRun,
+                                      wpi::ArrayRef<Subsystem*> requirements) {
   return WhileActiveContinous(InstantCommand(std::move(toRun), requirements));
 }
 
 Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (!pressedLast && pressed) {
           command->Schedule(interruptible);
@@ -73,8 +86,8 @@ Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
 
 Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (pressedLast && !pressed) {
           command->Schedule(interruptible);
@@ -87,13 +100,19 @@ Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
 
 Trigger Trigger::WhenInactive(std::function<void()> toRun,
                               std::initializer_list<Subsystem*> requirements) {
+  return WhenInactive(std::move(toRun), wpi::makeArrayRef(requirements.begin(),
+                                                          requirements.end()));
+}
+
+Trigger Trigger::WhenInactive(std::function<void()> toRun,
+                              wpi::ArrayRef<Subsystem*> requirements) {
   return WhenInactive(InstantCommand(std::move(toRun), requirements));
 }
 
 Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (!pressedLast && pressed) {
           if (command->IsScheduled()) {
@@ -110,8 +129,8 @@ Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
 
 Trigger Trigger::CancelWhenActive(Command* command) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command]() mutable {
+        bool pressed = m_isActive();
 
         if (!pressedLast && pressed) {
           command->Cancel();

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,6 +13,7 @@
 
 #include <frc/controller/ProfiledPIDController.h>
 #include <units/units.h>
+#include <wpi/ArrayRef.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -50,7 +51,29 @@ class ProfiledPIDCommand
                      std::function<Distance_t()> measurementSource,
                      std::function<State()> goalSource,
                      std::function<void(double, State)> useOutput,
-                     std::initializer_list<Subsystem*> requirements = {})
+                     std::initializer_list<Subsystem*> requirements)
+      : m_controller{controller},
+        m_measurement{std::move(measurementSource)},
+        m_goal{std::move(goalSource)},
+        m_useOutput{std::move(useOutput)} {
+    this->AddRequirements(requirements);
+  }
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goalSource   the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+                     std::function<Distance_t()> measurementSource,
+                     std::function<State()> goalSource,
+                     std::function<void(double, State)> useOutput,
+                     wpi::ArrayRef<Subsystem*> requirements = {})
       : m_controller{controller},
         m_measurement{std::move(measurementSource)},
         m_goal{std::move(goalSource)},
@@ -79,6 +102,27 @@ class ProfiledPIDCommand
                            },
                            useOutput, requirements) {}
 
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goalSource   the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+                     std::function<Distance_t()> measurementSource,
+                     std::function<Distance_t()> goalSource,
+                     std::function<void(double, State)> useOutput,
+                     wpi::ArrayRef<Subsystem*> requirements = {})
+      : ProfiledPIDCommand(controller, measurementSource,
+                           [&goalSource]() {
+                             return State{goalSource(), Velocity_t{0}};
+                           },
+                           useOutput, requirements) {}
+
   /**
    * Creates a new PIDCommand, which controls the given output with a
    * ProfiledPIDController with a constant goal.
@@ -96,6 +140,23 @@ class ProfiledPIDCommand
       : ProfiledPIDCommand(controller, measurementSource,
                            [goal] { return goal; }, useOutput, requirements) {}
 
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController with a constant goal.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goal         the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+                     std::function<Distance_t()> measurementSource, State goal,
+                     std::function<void(double, State)> useOutput,
+                     wpi::ArrayRef<Subsystem*> requirements = {})
+      : ProfiledPIDCommand(controller, measurementSource,
+                           [goal] { return goal; }, useOutput, requirements) {}
+
   /**
    * Creates a new PIDCommand, which controls the given output with a
    * ProfiledPIDController with a constant goal.
@@ -114,11 +175,29 @@ class ProfiledPIDCommand
       : ProfiledPIDCommand(controller, measurementSource,
                            [goal] { return goal; }, useOutput, requirements) {}
 
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController with a constant goal.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goal         the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+                     std::function<Distance_t()> measurementSource,
+                     Distance_t goal,
+                     std::function<void(double, State)> useOutput,
+                     wpi::ArrayRef<Subsystem*> requirements = {})
+      : ProfiledPIDCommand(controller, measurementSource,
+                           [goal] { return goal; }, useOutput, requirements) {}
+
   ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
 
   ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
 
-  void Initialize() override { m_controller.Reset(); }
+  void Initialize() override { m_controller.Reset(m_measurement()); }
 
   void Execute() override {
     m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -62,10 +62,10 @@ class ProfiledPIDSubsystem : public SubsystemBase {
   void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
 
   /**
-   * Enables the PID control.  Resets the controller.
+   * Enables the PID control. Resets the controller.
    */
   virtual void Enable() {
-    m_controller.Reset();
+    m_controller.Reset(GetMeasurement());
     m_enabled = true;
   }
 

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,6 +18,7 @@
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/trajectory/Trajectory.h>
 #include <units/units.h>
+#include <wpi/ArrayRef.h>
 
 #include "frc2/Timer.h"
 #include "frc2/command/CommandBase.h"
@@ -78,7 +79,44 @@ class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
                  frc2::PIDController leftController,
                  frc2::PIDController rightController,
                  std::function<void(units::volt_t, units::volt_t)> output,
-                 std::initializer_list<Subsystem*> requirements = {});
+                 std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the
+   * provided trajectory. PID control and feedforward are handled internally,
+   * and outputs are scaled -12 to 12 representing units of volts.
+   *
+   * <p>Note: The controller will *not* set the outputVolts to zero upon
+   * completion of the path - this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose - use one of
+   * the odometry classes to provide this.
+   * @param controller      The RAMSETE controller used to follow the
+   * trajectory.
+   * @param feedforward     A component for calculating the feedforward for the
+   * drive.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param wheelSpeeds     A function that supplies the speeds of the left
+   * and right sides of the robot drive.
+   * @param leftController  The PIDController for the left side of the robot
+   * drive.
+   * @param rightController The PIDController for the right side of the robot
+   * drive.
+   * @param output          A function that consumes the computed left and right
+   * outputs (in volts) for the robot drive.
+   * @param requirements    The subsystems to require.
+   */
+  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+                 frc::RamseteController controller,
+                 frc::SimpleMotorFeedforward<units::meters> feedforward,
+                 frc::DifferentialDriveKinematics kinematics,
+                 std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
+                 frc2::PIDController leftController,
+                 frc2::PIDController rightController,
+                 std::function<void(units::volt_t, units::volt_t)> output,
+                 wpi::ArrayRef<Subsystem*> requirements = {});
 
   /**
    * Constructs a new RamseteCommand that, when executed, will follow the
@@ -104,6 +142,30 @@ class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
                      output,
                  std::initializer_list<Subsystem*> requirements);
 
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the
+   * provided trajectory. Performs no PID control and calculates no
+   * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller,
+   * and will need to be converted into a usable form by the user.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose - use one of
+   * the odometry classes to provide this.
+   * @param controller      The RAMSETE controller used to follow the
+   * trajectory.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param output          A function that consumes the computed left and right
+   * wheel speeds.
+   * @param requirements    The subsystems to require.
+   */
+  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+                 frc::RamseteController controller,
+                 frc::DifferentialDriveKinematics kinematics,
+                 std::function<void(units::meters_per_second_t,
+                                    units::meters_per_second_t)>
+                     output,
+                 wpi::ArrayRef<Subsystem*> requirements = {});
+
   void Initialize() override;
 
   void Execute() override;

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

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

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,6 +10,8 @@
 #include <functional>
 #include <initializer_list>
 
+#include <wpi/ArrayRef.h>
+
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 
@@ -32,7 +34,18 @@ class StartEndCommand : public CommandHelper<CommandBase, StartEndCommand> {
    * @param requirements the subsystems required by this command
    */
   StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
-                  std::initializer_list<Subsystem*> requirements = {});
+                  std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new StartEndCommand.  Will run the given runnables when the
+   * command starts and when it ends.
+   *
+   * @param onInit       the Runnable to run on command init
+   * @param onEnd        the Runnable to run on command end
+   * @param requirements the subsystems required by this command
+   */
+  StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
+                  wpi::ArrayRef<Subsystem*> requirements = {});
 
   StartEndCommand(StartEndCommand&& other) = default;
 

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,7 @@
 
 #include <cmath>
 #include <functional>
+#include <initializer_list>
 #include <memory>
 
 #include <frc/controller/PIDController.h>
@@ -17,6 +18,7 @@
 #include <frc/kinematics/SwerveModuleState.h>
 #include <frc/trajectory/Trajectory.h>
 #include <units/units.h>
+#include <wpi/ArrayRef.h>
 
 #include "CommandBase.h"
 #include "CommandHelper.h"
@@ -92,6 +94,42 @@ class SwerveControllerCommand
           output,
       std::initializer_list<Subsystem*> requirements);
 
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the
+   * provided trajectory. This command will not return output voltages but
+   * rather raw module states from the position controllers which need to be put
+   * into a velocity PID.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path- this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The rotation controller will calculate the rotation based on the
+   * final pose in the trajectory, not the poses at each time step.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose,
+   *                        provided by the odometry class.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param xController     The Trajectory Tracker PID controller
+   *                        for the robot's x position.
+   * @param yController     The Trajectory Tracker PID controller
+   *                        for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller
+   *                        for angle for the robot.
+   * @param output          The raw output module states from the
+   *                        position controllers.
+   * @param requirements    The subsystems to require.
+   */
+  SwerveControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SwerveDriveKinematics<NumModules> kinematics,
+      frc2::PIDController xController, frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+          output,
+      wpi::ArrayRef<Subsystem*> requirements = {});
+
   void Initialize() override;
 
   void Execute() override;