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.

.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 {

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

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

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

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

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

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

@@ -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();
 }
@@ -521,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();
       }
     }
 
@@ -597,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/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/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/native/cpp/frc2/command/MecanumControllerCommand.cpp

@@ -247,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/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

@@ -138,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/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/include/frc2/command/SwerveControllerCommand.inc

@@ -103,7 +103,7 @@ void SwerveControllerCommand<NumModules>::End(bool interrupted) {
 
 template <size_t NumModules>
 bool SwerveControllerCommand<NumModules>::IsFinished() {
-  return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+  return m_timer.HasElapsed(m_trajectory.TotalTime());
 }
 
 }  // namespace frc2

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

@@ -72,7 +72,7 @@ class TrapezoidProfileCommand
   void End(bool interrupted) override { m_timer.Stop(); }
 
   bool IsFinished() override {
-    return m_timer.HasPeriodPassed(m_profile.TotalTime());
+    return m_timer.HasElapsed(m_profile.TotalTime());
   }
 
  private:

wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h

@@ -28,7 +28,7 @@ class POVButton : public Button {
    */
   POVButton(frc::GenericHID* joystick, int angle, int povNumber = 0)
       : Button([joystick, angle, povNumber] {
-          joystick->GetPOV(povNumber) == angle;
+          return joystick->GetPOV(povNumber) == angle;
         }) {}
 };
 }  // namespace frc2

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.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.                                                               */
@@ -15,6 +15,7 @@ import static org.junit.jupiter.api.Assertions.assertNotNull;
 import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.reset;
 import static org.mockito.Mockito.times;
 import static org.mockito.Mockito.verify;
 
@@ -160,4 +161,50 @@ class ParallelRaceGroupTest extends CommandTestBase {
 
   }
 
+  @Test
+  void parallelRaceScheduleTwiceTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelRaceGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    verify(command1).initialize();
+    verify(command2).initialize();
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+    command2Holder.setFinished(true);
+    scheduler.run();
+
+    verify(command1).execute();
+    verify(command1).end(false);
+    verify(command2).execute();
+    verify(command2).end(true);
+    verify(command2, never()).end(false);
+
+    assertFalse(scheduler.isScheduled(group));
+
+    reset(command1);
+    reset(command2);
+
+    scheduler.schedule(group);
+
+    verify(command1).initialize();
+    verify(command2).initialize();
+
+    scheduler.run();
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(group));
+    command2Holder.setFinished(true);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
 }

wpilibNewCommands/src/test/native/cpp/frc2/command/POVButtonTest.cpp

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/Joystick.h>
+#include <mockdata/DriverStationData.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/button/POVButton.h"
+#include "gtest/gtest.h"
+
+using namespace frc2;
+class POVButtonTest : public CommandTestBase {};
+
+TEST_F(POVButtonTest, SetPOVTest) {
+  HAL_JoystickPOVs povs;
+  povs.count = 1;
+  povs.povs[0] = 0;
+  HALSIM_SetJoystickPOVs(1, &povs);
+  HALSIM_NotifyDriverStationNewData();
+
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  frc::Joystick joy(1);
+  POVButton(&joy, 90).WhenPressed(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  povs.povs[0] = 90;
+  HALSIM_SetJoystickPOVs(1, &povs);
+  HALSIM_NotifyDriverStationNewData();
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.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.                                                               */
@@ -154,3 +154,54 @@ TEST_F(ParallelRaceGroupTest, ParallelRaceOnlyCallsEndOnceTest) {
   EXPECT_NO_FATAL_FAILURE(scheduler.Run());
   EXPECT_FALSE(scheduler.IsScheduled(&group2));
 }
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTwiceTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize()).Times(2);
+  EXPECT_CALL(*command1, Execute()).Times(5);
+  EXPECT_CALL(*command1, End(true)).Times(2);
+
+  EXPECT_CALL(*command2, Initialize()).Times(2);
+  EXPECT_CALL(*command2, Execute()).Times(5);
+  EXPECT_CALL(*command2, End(false)).Times(2);
+
+  EXPECT_CALL(*command3, Initialize()).Times(2);
+  EXPECT_CALL(*command3, Execute()).Times(5);
+  EXPECT_CALL(*command3, End(true)).Times(2);
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+
+  command2->SetFinished(false);
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.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.                                                               */
@@ -66,7 +66,7 @@ public abstract class PIDSubsystem extends Subsystem {
   }
 
   /**
-   * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+   * Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values.
    *
    * @param name the name
    * @param p    the proportional value
@@ -82,7 +82,7 @@ public abstract class PIDSubsystem extends Subsystem {
   }
 
   /**
-   * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will also
+   * Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values. It will also
    * space the time between PID loop calculations to be equal to the given period.
    *
    * @param name   the name
@@ -114,9 +114,9 @@ public abstract class PIDSubsystem extends Subsystem {
   }
 
   /**
-   * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will use the
-   * class name as its name. It will also space the time between PID loop calculations to be equal
-   * to the given period.
+   * Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values. It will use
+   * the class name as its name. It will also space the time between PID loop calculations to be
+   * equal to the given period.
    *
    * @param p      the proportional value
    * @param i      the integral value

wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-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,7 @@ class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource {
   PIDSubsystem(const wpi::Twine& name, double p, double i, double d);
 
   /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   * Instantiates a PIDSubsystem that will use the given P, I, D, and F values.
    *
    * @param name the name
    * @param p    the proportional value
@@ -51,7 +51,7 @@ class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource {
   PIDSubsystem(const wpi::Twine& name, double p, double i, double d, double f);
 
   /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   * Instantiates a PIDSubsystem that will use the given P, I, D, and F values.
    *
    * It will also space the time between PID loop calculations to be equal to
    * the given period.
@@ -78,7 +78,7 @@ class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource {
   PIDSubsystem(double p, double i, double d);
 
   /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   * Instantiates a PIDSubsystem that will use the given P, I, D, and F values.
    *
    * It will use the class name as its name.
    *
@@ -90,7 +90,7 @@ class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource {
   PIDSubsystem(double p, double i, double d, double f);
 
   /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   * Instantiates a PIDSubsystem that will use the given P, I, D, and F values.
    *
    * It will use the class name as its name. It will also space the time
    * between PID loop calculations to be equal to the given period.

wpilibc/src/main/native/cpp/ADXL345_I2C.cpp

@@ -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.                                                               */
@@ -21,7 +21,7 @@ ADXL345_I2C::ADXL345_I2C(I2C::Port port, Range range, int deviceAddress)
     m_simRange =
         m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
     m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
-    m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+    m_simY = m_simDevice.CreateDouble("Y Accel", false, 0.0);
     m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
   }
   // Turn on the measurements

wpilibc/src/main/native/cpp/ADXL345_SPI.cpp

@@ -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.                                                               */
@@ -20,7 +20,7 @@ ADXL345_SPI::ADXL345_SPI(SPI::Port port, ADXL345_SPI::Range range)
     m_simRange =
         m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
     m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
-    m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+    m_simY = m_simDevice.CreateDouble("Y Accel", false, 0.0);
     m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
   }
   m_spi.SetClockRate(500000);

wpilibc/src/main/native/cpp/ADXL362.cpp

@@ -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.                                                               */
@@ -40,7 +40,7 @@ ADXL362::ADXL362(SPI::Port port, Range range)
     m_simRange =
         m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
     m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
-    m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+    m_simY = m_simDevice.CreateDouble("Y Accel", false, 0.0);
     m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
   }
 

wpilibc/src/main/native/cpp/Filesystem.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 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.                                                               */
@@ -18,7 +18,7 @@ void frc::filesystem::GetLaunchDirectory(wpi::SmallVectorImpl<char>& result) {
 
 void frc::filesystem::GetOperatingDirectory(
     wpi::SmallVectorImpl<char>& result) {
-  if (RobotBase::IsReal()) {
+  if constexpr (RobotBase::IsReal()) {
     wpi::sys::path::native("/home/lvuser", result);
   } else {
     frc::filesystem::GetLaunchDirectory(result);
@@ -27,5 +27,11 @@ void frc::filesystem::GetOperatingDirectory(
 
 void frc::filesystem::GetDeployDirectory(wpi::SmallVectorImpl<char>& result) {
   frc::filesystem::GetOperatingDirectory(result);
-  wpi::sys::path::append(result, "deploy");
+  if constexpr (RobotBase::IsReal()) {
+    wpi::sys::path::append(result, "deploy");
+  } else {
+    wpi::sys::path::append(result, "src");
+    wpi::sys::path::append(result, "main");
+    wpi::sys::path::append(result, "deploy");
+  }
 }

wpilibc/src/main/native/cpp/IterativeRobot.cpp

@@ -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,10 @@ IterativeRobot::IterativeRobot() : IterativeRobotBase(kPacketPeriod) {
 void IterativeRobot::StartCompetition() {
   RobotInit();
 
+  if (IsSimulation()) {
+    SimulationInit();
+  }
+
   // Tell the DS that the robot is ready to be enabled
   HAL_ObserveUserProgramStarting();
 

wpilibc/src/main/native/cpp/IterativeRobotBase.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.                                                               */
@@ -34,6 +34,10 @@ void IterativeRobotBase::RobotInit() {
   wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
 }
 
+void IterativeRobotBase::SimulationInit() {
+  wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+}
+
 void IterativeRobotBase::DisabledInit() {
   wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
 }
@@ -58,6 +62,14 @@ void IterativeRobotBase::RobotPeriodic() {
   }
 }
 
+void IterativeRobotBase::SimulationPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+    firstRun = false;
+  }
+}
+
 void IterativeRobotBase::DisabledPeriodic() {
   static bool firstRun = true;
   if (firstRun) {
@@ -161,6 +173,12 @@ void IterativeRobotBase::LoopFunc() {
   m_watchdog.AddEpoch("LiveWindow::UpdateValues()");
   Shuffleboard::Update();
   m_watchdog.AddEpoch("Shuffleboard::Update()");
+
+  if (IsSimulation()) {
+    SimulationPeriodic();
+    m_watchdog.AddEpoch("SimulationPeriodic()");
+  }
+
   m_watchdog.Disable();
 
   // Warn on loop time overruns

wpilibc/src/main/native/cpp/Notifier.cpp

@@ -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.                                                               */
@@ -129,6 +129,8 @@ void Notifier::StartPeriodic(units::second_t period) {
 }
 
 void Notifier::Stop() {
+  std::scoped_lock lock(m_processMutex);
+  m_periodic = false;
   int32_t status = 0;
   HAL_CancelNotifierAlarm(m_notifier, &status);
   wpi_setHALError(status);

wpilibc/src/main/native/cpp/TimedRobot.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.                                                               */
@@ -24,6 +24,10 @@ using namespace frc;
 void TimedRobot::StartCompetition() {
   RobotInit();
 
+  if (IsSimulation()) {
+    SimulationInit();
+  }
+
   // Tell the DS that the robot is ready to be enabled
   HAL_ObserveUserProgramStarting();
 

wpilibc/src/main/native/cpp/controller/RamseteController.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.                                                               */
@@ -45,6 +45,10 @@ ChassisSpeeds RamseteController::Calculate(
     const Pose2d& currentPose, const Pose2d& poseRef,
     units::meters_per_second_t linearVelocityRef,
     units::radians_per_second_t angularVelocityRef) {
+  if (!m_enabled) {
+    return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
+  }
+
   m_poseError = poseRef.RelativeTo(currentPose);
 
   // Aliases for equation readability
@@ -68,3 +72,5 @@ ChassisSpeeds RamseteController::Calculate(
   return Calculate(currentPose, desiredState.pose, desiredState.velocity,
                    desiredState.velocity * desiredState.curvature);
 }
+
+void RamseteController::SetEnabled(bool enabled) { m_enabled = enabled; }

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

@@ -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.                                                               */
@@ -116,15 +116,22 @@ void Timer::Stop() {
   }
 }
 
+bool Timer::HasElapsed(units::second_t period) { return Get() > period; }
+
 bool Timer::HasPeriodPassed(units::second_t period) {
+  return AdvanceIfElapsed(period);
+}
+
+bool Timer::AdvanceIfElapsed(units::second_t period) {
   if (Get() > period) {
     std::scoped_lock lock(m_mutex);
     // Advance the start time by the period.
     m_startTime += period;
     // Don't set it to the current time... we want to avoid drift.
     return true;
+  } else {
+    return false;
   }
-  return false;
 }
 
 units::second_t Timer::GetFPGATimestamp() {

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

@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/Field2d.h"
+
+using namespace frc;
+
+Field2d::Field2d() : m_device{"Field2D"} {
+  if (m_device) {
+    m_x = m_device.CreateDouble("x", false, 0.0);
+    m_y = m_device.CreateDouble("y", false, 0.0);
+    m_rot = m_device.CreateDouble("rot", false, 0.0);
+  }
+}
+
+void Field2d::SetRobotPose(const Pose2d& pose) {
+  if (m_device) {
+    auto& translation = pose.Translation();
+    m_x.Set(translation.X().to<double>());
+    m_y.Set(translation.Y().to<double>());
+    m_rot.Set(pose.Rotation().Degrees().to<double>());
+  } else {
+    m_pose = pose;
+  }
+}
+
+void Field2d::SetRobotPose(units::meter_t x, units::meter_t y,
+                           Rotation2d rotation) {
+  if (m_device) {
+    m_x.Set(x.to<double>());
+    m_y.Set(y.to<double>());
+    m_rot.Set(rotation.Degrees().to<double>());
+  } else {
+    m_pose = Pose2d{x, y, rotation};
+  }
+}
+
+Pose2d Field2d::GetRobotPose() {
+  if (m_device) {
+    return Pose2d{units::meter_t{m_x.Get()}, units::meter_t{m_y.Get()},
+                  Rotation2d{units::degree_t{m_rot.Get()}}};
+  } else {
+    return m_pose;
+  }
+}

wpilibc/src/main/native/cpp/spline/SplineHelper.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.                                                               */
@@ -166,7 +166,12 @@ SplineHelper::QuinticControlVectorsFromWaypoints(
     const auto scalar =
         1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
 
-    vectors.push_back(QuinticControlVector(scalar, p0));
+    // Only add the first control vector if this is the first iteration. The
+    // last control vector of this iteration is the first control vector of
+    // the next iteration.
+    if (i == 0) {
+      vectors.push_back(QuinticControlVector(scalar, p0));
+    }
     vectors.push_back(QuinticControlVector(scalar, p1));
   }
   return vectors;

wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.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.                                                               */
@@ -47,30 +47,51 @@ DifferentialDriveVoltageConstraint::MinMaxAcceleration(
   // Robot chassis turning on radius = 1/|curvature|.  Outer wheel has radius
   // increased by half of the trackwidth T.  Inner wheel has radius decreased
   // by half of the trackwidth.  Achassis / radius = Aouter / (radius + T/2), so
-  // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2)
-  // Inner wheel is similar.
+  // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 +
+  // |curvature|T/2). Inner wheel is similar.
 
   // sgn(speed) term added to correctly account for which wheel is on
   // outside of turn:
   // If moving forward, max acceleration constraint corresponds to wheel on
-  // outside of turn
-  // If moving backward, max acceleration constraint corresponds to wheel on
-  // inside of turn
-  auto maxChassisAcceleration =
-      maxWheelAcceleration /
-      (1 + m_kinematics.trackWidth * units::math::abs(curvature) *
-               wpi::sgn(speed) / (2_rad));
-  auto minChassisAcceleration =
-      minWheelAcceleration /
-      (1 - m_kinematics.trackWidth * units::math::abs(curvature) *
-               wpi::sgn(speed) / (2_rad));
+  // outside of turn If moving backward, max acceleration constraint corresponds
+  // to wheel on inside of turn
+
+  // When velocity is zero, then wheel velocities are uniformly zero (robot
+  // cannot be turning on its center) - we have to treat this as a special case,
+  // as it breaks the signum function.  Both max and min acceleration are
+  // *reduced in magnitude* in this case.
+
+  units::meters_per_second_squared_t maxChassisAcceleration;
+  units::meters_per_second_squared_t minChassisAcceleration;
+
+  if (speed == 0_mps) {
+    maxChassisAcceleration =
+        maxWheelAcceleration /
+        (1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
+    minChassisAcceleration =
+        minWheelAcceleration /
+        (1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
+  } else {
+    maxChassisAcceleration =
+        maxWheelAcceleration /
+        (1 + m_kinematics.trackWidth * units::math::abs(curvature) *
+                 wpi::sgn(speed) / (2_rad));
+    minChassisAcceleration =
+        minWheelAcceleration /
+        (1 - m_kinematics.trackWidth * units::math::abs(curvature) *
+                 wpi::sgn(speed) / (2_rad));
+  }
+
+  // When turning about a point inside of the wheelbase (i.e. radius less than
+  // half the trackwidth), the inner wheel's direction changes, but the
+  // magnitude remains the same.  The formula above changes sign for the inner
+  // wheel when this happens. We can accurately account for this by simply
+  // negating the inner wheel.
 
-  // Negate acceleration corresponding to wheel on inside of turn
-  // if center of turn is inside of wheelbase
   if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) {
     if (speed > 0_mps) {
       minChassisAcceleration = -minChassisAcceleration;
-    } else {
+    } else if (speed < 0_mps) {
       maxChassisAcceleration = -maxChassisAcceleration;
     }
   }

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

@@ -89,8 +89,6 @@ static void SetupCameraServerShared() {
   if (symbol) {
     auto setCameraServerShared = (SetCameraServerSharedFP)symbol;
     setCameraServerShared(new WPILibCameraServerShared{});
-    wpi::outs() << "Set Camera Server Shared\n";
-    wpi::outs().flush();
   } else {
     wpi::outs() << "Camera Server Shared Symbol Missing\n";
     wpi::outs().flush();

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-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.                                                               */
@@ -27,6 +27,7 @@ namespace frc {
  * determine the heading.
  *
  * This class is for the digital ADXRS450 gyro sensor that connects via SPI.
+ * Only one instance of an ADXRS Gyro is supported.
  */
 class ADXRS450_Gyro : public GyroBase {
  public:

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

@@ -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.                                                               */
@@ -34,7 +34,7 @@ void GetOperatingDirectory(wpi::SmallVectorImpl<char>& result);
  * Obtains the deploy directory of the program, which is the remote location
  * src/main/deploy is deployed to by default. On the roboRIO, this is
  * /home/lvuser/deploy. In simulation, it is where the simulation was launched
- * from, in the subdirectory "deploy" (`pwd`/deploy).
+ * from, in the subdirectory "src/main/deploy" (`pwd`/src/main/deploy).
  *
  * @param result The result of the operating directory lookup
  */

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

@@ -61,6 +61,16 @@ class IterativeRobotBase : public RobotBase {
    */
   virtual void RobotInit();
 
+  /**
+   * Robot-wide simulation initialization code should go here.
+   *
+   * Users should override this method for default Robot-wide simulation
+   * related initialization which will be called when the robot is first
+   * started. It will be called exactly one time after RobotInit is called
+   * only when the robot is in simulation.
+   */
+  virtual void SimulationInit();
+
   /**
    * Initialization code for disabled mode should go here.
    *
@@ -102,6 +112,13 @@ class IterativeRobotBase : public RobotBase {
    */
   virtual void RobotPeriodic();
 
+  /**
+   * Periodic simulation code should go here.
+   *
+   * This function is called in a simulated robot after user code executes.
+   */
+  virtual void SimulationPeriodic();
+
   /**
    * Periodic code for disabled mode should go here.
    *

wpilibc/src/main/native/include/frc/controller/RamseteController.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.                                                               */
@@ -102,12 +102,20 @@ class RamseteController {
   ChassisSpeeds Calculate(const Pose2d& currentPose,
                           const Trajectory::State& desiredState);
 
+  /**
+   * Enables and disables the controller for troubleshooting purposes.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  void SetEnabled(bool enabled);
+
  private:
   double m_b;
   double m_zeta;
 
   Pose2d m_poseError;
   Pose2d m_poseTolerance;
+  bool m_enabled = true;
 };
 
 }  // namespace frc

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

@@ -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.                                                               */
@@ -144,7 +144,7 @@ enum class BuiltInWidgets {
    * <br>Supported types:
    * <ul>
    * <li>Number</li>
-   * <li>{@link edu.wpi.first.wpilibj.AnalogInput}</li>
+   * <li>AnalogInput</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -163,9 +163,8 @@ enum class BuiltInWidgets {
    */
   kVoltageView,
   /**
-   * Displays a {@link edu.wpi.first.wpilibj.PowerDistributionPanel
-   * PowerDistributionPanel}. <br>Supported types: <ul> <li>{@link
-   * edu.wpi.first.wpilibj.PowerDistributionPanel}</li>
+   * Displays a PowerDistributionPanel. <br>Supported types: <ul> <li>
+   * PowerDistributionPanel</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -176,49 +175,49 @@ enum class BuiltInWidgets {
    */
   kPowerDistributionPanel,
   /**
-   * Displays a {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser
-   * SendableChooser} with a dropdown combo box with a list of options.
+   * Displays a SendableChooser with a dropdown combo box with a list of
+   * options.
    * <br>Supported types:
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser}</li>
+   * <li>SendableChooser</li>
    * </ul>
    * <br>This widget has no custom properties.
    */
   kComboBoxChooser,
   /**
-   * Displays a {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser
-   * SendableChooser} with a toggle button for each available option.
+   * Displays a SendableChooserwith a toggle button for each available option.
    * <br>Supported types:
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser}</li>
+   * <li>SendableChooser</li>
    * </ul>
    * <br>This widget has no custom properties.
    */
   kSplitButtonChooser,
   /**
-   * Displays an {@link edu.wpi.first.wpilibj.Encoder} displaying its speed,
-   * total travelled distance, and its distance per tick. <br>Supported types:
+   * Displays an Encoder displaying its speed,
+   * total traveled distance, and its distance per tick. <br>Supported types:
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.Encoder}</li>
+   * <li>Encoder</li>
    * </ul>
    * <br>This widget has no custom properties.
    */
   kEncoder,
   /**
-   * Displays a {@link edu.wpi.first.wpilibj.SpeedController SpeedController}.
+   * Displays a SpeedController.
    * The speed controller will be controllable from the dashboard when test mode
    * is enabled, but will otherwise be view-only. <br>Supported types: <ul>
-   * <li>{@link edu.wpi.first.wpilibj.PWMSpeedController}</li>
-   * <li>{@link edu.wpi.first.wpilibj.DMC60}</li>
-   * <li>{@link edu.wpi.first.wpilibj.Jaguar}</li>
-   * <li>{@link edu.wpi.first.wpilibj.PWMTalonSRX}</li>
-   * <li>{@link edu.wpi.first.wpilibj.PWMVictorSPX}</li>
-   * <li>{@link edu.wpi.first.wpilibj.SD540}</li>
-   * <li>{@link edu.wpi.first.wpilibj.Spark}</li>
-   * <li>{@link edu.wpi.first.wpilibj.Talon}</li>
-   * <li>{@link edu.wpi.first.wpilibj.Victor}</li>
-   * <li>{@link edu.wpi.first.wpilibj.VictorSP}</li>
-   * <li>{@link edu.wpi.first.wpilibj.SpeedControllerGroup}</li>
+   * <li>PWMSpeedController</li>
+   * <li>DMC60</li>
+   * <li>Jaguar</li>
+   * <li>PWMTalonSRX</li>
+   * <li>PWMVictorSPX</li>
+   * <li>SD540</li>
+   * <li>Spark</li>
+   * <li>Talon</li>
+   * <li>Victor</li>
+   * <li>VictorSP</li>
+   * <li>SpeedControllerGroup</li>
+   * <li>Any custom subclass of {@code SpeedContorller}</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -231,10 +230,8 @@ enum class BuiltInWidgets {
   /**
    * Displays a command with a toggle button. Pressing the button will start the
    * command, and the button will automatically release when the command
-   * completes. <br>Supported types: <ul> <li>{@link
-   * edu.wpi.first.wpilibj.command.Command}</li> <li>{@link
-   * edu.wpi.first.wpilibj.command.CommandGroup}</li> <li>Any custom subclass of
-   * {@code Command} or {@code CommandGroup}</li>
+   * completes. <br>Supported types: <ul> <li>Command</li> <li>CommandGroup</li>
+   * <li>Any custom subclass of {@code Command} or {@code CommandGroup}</li>
    * </ul>
    * <br>This widget has no custom properties.
    */
@@ -243,7 +240,7 @@ enum class BuiltInWidgets {
    * Displays a PID command with a checkbox and an editor for the PIDF
    * constants. Selecting the checkbox will start the command, and the checkbox
    * will automatically deselect when the command completes. <br>Supported
-   * types: <ul> <li>{@link edu.wpi.first.wpilibj.command.PIDCommand}</li>
+   * types: <ul> <li>PIDCommand</li>
    * <li>Any custom subclass of {@code PIDCommand}</li>
    * </ul>
    * <br>This widget has no custom properties.
@@ -252,7 +249,7 @@ enum class BuiltInWidgets {
   /**
    * Displays a PID controller with an editor for the PIDF constants and a
    * toggle switch for enabling and disabling the controller. <br>Supported
-   * types: <ul> <li>{@link edu.wpi.first.wpilibj.PIDController}</li>
+   * types: <ul> <li>PIDController</li>
    * </ul>
    * <br>This widget has no custom properties.
    */
@@ -260,7 +257,7 @@ enum class BuiltInWidgets {
   /**
    * Displays an accelerometer with a number bar displaying the magnitude of the
    * acceleration and text displaying the exact value. <br>Supported types: <ul>
-   * <li>{@link edu.wpi.first.wpilibj.AnalogAccelerometer}</li>
+   * <li>AnalogAccelerometer</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -280,10 +277,8 @@ enum class BuiltInWidgets {
   kAccelerometer,
   /**
    * Displays a 3-axis accelerometer with a number bar for each axis'
-   * accleration. <br>Supported types: <ul> <li>{@link
-   * edu.wpi.first.wpilibj.ADXL345_I2C}</li> <li>{@link
-   * edu.wpi.first.wpilibj.ADXL345_SPI}</li> <li>{@link
-   * edu.wpi.first.wpilibj.ADXL362}</li>
+   * acceleration. <br>Supported types: <ul> <li>ADXL345_I2C</li> <li>
+   * ADXL345_SPI</li> <li>ADXL362</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -302,8 +297,8 @@ enum class BuiltInWidgets {
    * Displays a gyro with a dial from 0 to 360 degrees.
    * <br>Supported types:
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.ADXRS450_Gyro}</li>
-   * <li>{@link edu.wpi.first.wpilibj.AnalogGyro}</li>
+   * <li>ADXRS450_Gyro</li>
+   * <li>AnalogGyro</li>
    * <li>Any custom subclass of {@code GyroBase} (such as a MXP gyro)</li>
    * </ul>
    * <br>Custom properties:
@@ -319,8 +314,7 @@ enum class BuiltInWidgets {
   kGyro,
   /**
    * Displays a relay with toggle buttons for each supported mode (off, on,
-   * forward, reverse). <br>Supported types: <ul> <li>{@link
-   * edu.wpi.first.wpilibj.Relay}</li>
+   * forward, reverse). <br>Supported types: <ul> <li>Relay</li>
    * </ul>
    * <br>This widget has no custom properties.
    */
@@ -331,7 +325,7 @@ enum class BuiltInWidgets {
    * drivebase. The widget will be controllable if the robot is in test mode.
    * <br>Supported types:
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.drive.DifferentialDrive}</li>
+   * <li>DifferentialDrive</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -348,7 +342,7 @@ enum class BuiltInWidgets {
    * Displays a mecanum drive with a widget that displays the speed of each
    * wheel, and vectors for the direction and rotation of the drivebase. The
    * widget will be controllable if the robot is in test mode. <br>Supported
-   * types: <ul> <li>{@link edu.wpi.first.wpilibj.drive.MecanumDrive}</li>
+   * types: <ul> <li>MecanumDrive</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -361,8 +355,7 @@ enum class BuiltInWidgets {
    * Displays a camera stream.
    * <br>Supported types:
    * <ul>
-   * <li>{@link edu.wpi.cscore.VideoSource} (as long as it is streaming on an
-   * MJPEG server)</li>
+   * <li>VideoSource (as long as it is streaming on an MJPEG server)</li>
    * </ul>
    * <br>Custom properties:
    * <table>

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 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.                                                               */
@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <wpi/StringRef.h>
+
 namespace frc {
 
 // Maintainer note: this enum is mirrored in WPILibJ and in Shuffleboard

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

@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <hal/SimDevice.h>
+#include <units/units.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+
+/**
+ * 2D representation of game field (for simulation).
+ *
+ * In non-simulation mode this simply stores and returns the robot pose.
+ *
+ * The robot pose is the actual location shown on the simulation view.
+ * This may or may not match the robot's internal odometry.  For example, if
+ * the robot is shown at a particular starting location, the pose in this
+ * class would represent the actual location on the field, but the robot's
+ * internal state might have a 0,0,0 pose (unless it's initialized to
+ * something different).
+ *
+ * As the user is able to edit the pose, code performing updates should get
+ * the robot pose, transform it as appropriate (e.g. based on simulated wheel
+ * velocity), and set the new pose.
+ */
+class Field2d {
+ public:
+  Field2d();
+
+  /**
+   * Set the robot pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  void SetRobotPose(const Pose2d& pose);
+
+  /**
+   * Set the robot pose from x, y, and rotation.
+   *
+   * @param x X location
+   * @param y Y location
+   * @param rotation rotation
+   */
+  void SetRobotPose(units::meter_t x, units::meter_t y, Rotation2d rotation);
+
+  /**
+   * Get the robot pose.
+   *
+   * @return 2D pose
+   */
+  Pose2d GetRobotPose();
+
+ private:
+  Pose2d m_pose;
+
+  hal::SimDevice m_device;
+  hal::SimDouble m_x;
+  hal::SimDouble m_y;
+  hal::SimDouble m_rot;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.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.                                                               */
@@ -43,6 +43,7 @@ namespace frc {
  */
 template <class Distance>
 class TrapezoidProfile {
+ public:
   using Distance_t = units::unit_t<Distance>;
   using Velocity =
       units::compound_unit<Distance, units::inverse<units::seconds>>;
@@ -51,7 +52,6 @@ class TrapezoidProfile {
       units::compound_unit<Velocity, units::inverse<units::seconds>>;
   using Acceleration_t = units::unit_t<Acceleration>;
 
- public:
   class Constraints {
    public:
     Constraints() {

wpilibc/src/main/native/include/frc2/Timer.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.                                                               */
@@ -88,6 +88,14 @@ class Timer {
    */
   void Stop();
 
+  /**
+   * Check if the period specified has passed.
+   *
+   * @param seconds The period to check.
+   * @return        True if the period has passed.
+   */
+  bool HasElapsed(units::second_t period);
+
   /**
    * Check if the period specified has passed and if it has, advance the start
    * time by that period. This is useful to decide if it's time to do periodic
@@ -98,6 +106,16 @@ class Timer {
    */
   bool HasPeriodPassed(units::second_t period);
 
+  /**
+   * Check if the period specified has passed and if it has, advance the start
+   * time by that period. This is useful to decide if it's time to do periodic
+   * work without drifting later by the time it took to get around to checking.
+   *
+   * @param period The period to check for.
+   * @return       True if the period has passed.
+   */
+  bool AdvanceIfElapsed(units::second_t period);
+
   /**
    * Return the FPGA system clock time in seconds.
    *

wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.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.                                                               */
@@ -11,7 +11,9 @@
 
 #include <units/units.h>
 
+#include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
 #include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
 #include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
@@ -57,3 +59,26 @@ TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
                 -maxVoltage - 0.05_V);
   }
 }
+
+TEST(DifferentialDriveVoltageConstraintTest, HighCurvature) {
+  SimpleMotorFeedforward<units::meter> feedforward{1_V, 1_V / 1_mps,
+                                                   3_V / 1_mps_sq};
+  // Large trackwidth - need to test with radius of curvature less than half of
+  // trackwidth
+  const DifferentialDriveKinematics kinematics{3_m};
+  const auto maxVoltage = 10_V;
+
+  auto config = TrajectoryConfig(12_fps, 12_fps_sq);
+  config.AddConstraint(
+      DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage));
+
+  EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
+      Pose2d{1_m, 0_m, Rotation2d{90_deg}}, std::vector<Translation2d>{},
+      Pose2d{0_m, 1_m, Rotation2d{180_deg}}, config));
+
+  config.SetReversed(true);
+
+  EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
+      Pose2d{0_m, 1_m, Rotation2d{180_deg}}, std::vector<Translation2d>{},
+      Pose2d{1_m, 0_m, Rotation2d{90_deg}}, config));
+}

wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.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.                                                               */
@@ -8,13 +8,15 @@
 #include "Drivetrain.h"
 
 void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
-  const auto leftOutput = m_leftPIDController.Calculate(
+  const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
+  const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
+  const double leftOutput = m_leftPIDController.Calculate(
       m_leftEncoder.GetRate(), speeds.left.to<double>());
-  const auto rightOutput = m_rightPIDController.Calculate(
+  const double rightOutput = m_rightPIDController.Calculate(
       m_rightEncoder.GetRate(), speeds.right.to<double>());
 
-  m_leftGroup.Set(leftOutput);
-  m_rightGroup.Set(rightOutput);
+  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
+  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
 }
 
 void Drivetrain::Drive(units::meters_per_second_t xSpeed,

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

@@ -1,10 +1,11 @@
 /*----------------------------------------------------------------------------*/
-/* 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.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <frc/SlewRateLimiter.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 
@@ -20,14 +21,16 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    const auto xSpeed =
-        -m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
+    const auto xSpeed = -m_speedLimiter.Calculate(
+                            m_controller.GetY(frc::GenericHID::kLeftHand)) *
+                        Drivetrain::kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-    const auto rot = -m_controller.GetX(frc::GenericHID::kRightHand) *
+    const auto rot = -m_rotLimiter.Calculate(
+                         m_controller.GetX(frc::GenericHID::kRightHand)) *
                      Drivetrain::kMaxAngularSpeed;
 
     m_drive.Drive(xSpeed, rot);
@@ -35,6 +38,12 @@ class Robot : public frc::TimedRobot {
 
  private:
   frc::XboxController m_controller{0};
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+  // to 1.
+  frc::SlewRateLimiter<units::scalar> m_speedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
+
   Drivetrain m_drive;
 };
 

wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.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/PWMVictorSPX.h>
 #include <frc/SpeedControllerGroup.h>
 #include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
 #include <units/units.h>
@@ -77,4 +78,8 @@ class Drivetrain {
 
   frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
   frc::DifferentialDriveOdometry m_odometry{GetAngle()};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
 };

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -25,11 +25,6 @@ RobotContainer::RobotContainer()
   frc::SmartDashboard::PutData(&m_wrist);
   frc::SmartDashboard::PutData(&m_claw);
 
-  m_claw.Log();
-  m_wrist.Log();
-  m_elevator.Log();
-  m_drivetrain.Log();
-
   m_drivetrain.SetDefaultCommand(TankDrive(
       [this] { return m_joy.GetY(frc::GenericHID::JoystickHand::kLeftHand); },
       [this] { return m_joy.GetY(frc::GenericHID::JoystickHand::kRightHand); },

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.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.                                                               */
@@ -7,6 +7,8 @@
 
 #include "subsystems/Claw.h"
 
+#include <frc/smartdashboard/SmartDashboard.h>
+
 Claw::Claw() {
   // Let's show everything on the LiveWindow
   SetName("Claw");
@@ -21,4 +23,8 @@ void Claw::Stop() { m_motor.Set(0); }
 
 bool Claw::IsGripping() { return m_contact.Get(); }
 
-void Claw::Log() {}
+void Claw::Log() {
+  frc::SmartDashboard::PutBoolean("Claw switch", IsGripping());
+}
+
+void Claw::Periodic() { Log(); }

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.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.                                                               */
@@ -67,3 +67,5 @@ double DriveTrain::GetDistanceToObstacle() {
   // Really meters in simulation since it's a rangefinder...
   return m_rangefinder.GetAverageVoltage();
 }
+
+void DriveTrain::Periodic() { Log(); }