[wpimath] Move math functionality into new wpimath library (#2629)

The wpimath library is a new library designed to separate the reusable math functionality from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j). Package names / include file names were NOT changed to minimize breakage. In a future year it would be good to revamp these for a more uniform user experience and to reduce the risk of accidental naming conflicts. While theoretically all of this functionality could be placed into wpiutil, several pieces of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this expense for users who only want cscore or ntcore. It also allows for easy future separation of build tasks vs number of workers on memory-constrained machines. This moves the following functionality from wpiutil into wpimath: - Eigen - ejml - Drake - DARE - wpiutil.math package (Matrix etc) - units And the following functionality from wpilibc/j into wpimath: - Geometry - Kinematics - Spline - Trajectory - LinearFilter - MedianFilter - Feed-forward controllers
2026-06-19 00:41:43 +00:00 · 2020-08-06 23:57:39 -07:00
parent ad817d4f23
commit 42993b15c6
463 changed files with 1006 additions and 399 deletions
--- a/.styleguide
+++ b/.styleguide
@@ -20,17 +20,20 @@ repoRootNameOverride {
 }

 includeOtherLibs {
+  ^Eigen/
  ^cameraserver/
  ^cscore
  ^drake/
  ^hal/
  ^imgui
+  ^math/
  ^mockdata/
  ^networktables/
  ^ntcore
  ^opencv2/
  ^support/
  ^units/
+  ^unsupported/
  ^vision/
  ^wpi/
 }
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -96,6 +96,7 @@ set(NTCORE_DEP_REPLACE "include($\{SELF_DIR\}/ntcore-config.cmake)")
 set(CSCORE_DEP_REPLACE_IMPL "include(\${SELF_DIR}/cscore-config.cmake)")
 set(CAMERASERVER_DEP_REPLACE_IMPL "include(\${SELF_DIR}/cameraserver-config.cmake)")
 set(HAL_DEP_REPLACE_IMPL "include(\${SELF_DIR}/hal-config.cmake)")
+set(WPIMATH_DEP_REPLACE "include($\{SELF_DIR\}/wpimath-config.cmake)")
 set(WPILIBC_DEP_REPLACE_IMPL "include(\${SELF_DIR}/wpilibc-config.cmake)")
 else()
 set(WPIUTIL_DEP_REPLACE "find_dependency(wpiutil)")
@@ -103,6 +104,7 @@ set(NTCORE_DEP_REPLACE "find_dependency(ntcore)")
 set(CSCORE_DEP_REPLACE_IMPL "find_dependency(cscore)")
 set(CAMERASERVER_DEP_REPLACE_IMPL "find_dependency(cameraserver)")
 set(HAL_DEP_REPLACE_IMPL "find_dependency(hal)")
+set(WPIMATH_DEP_REPLACE "find_dependency(wpimath)")
 set(WPILIBC_DEP_REPLACE_IMPL "find_dependency(wpilibc)")
 endif()

@@ -127,6 +129,7 @@ if (NOT WITHOUT_CSCORE)
        set(HAL_DEP_REPLACE ${HAL_DEP_REPLACE_IMPL})
        set(WPILIBC_DEP_REPLACE ${WPILIBC_DEP_REPLACE_IMPL})
        add_subdirectory(hal)
+        add_subdirectory(wpimath)
        add_subdirectory(wpilibj)
        add_subdirectory(wpilibc)
        add_subdirectory(myRobot)
--- a/docs/build.gradle
+++ b/docs/build.gradle
@@ -8,6 +8,7 @@ evaluationDependsOn(':ntcore')
 evaluationDependsOn(':cscore')
 evaluationDependsOn(':hal')
 evaluationDependsOn(':cameraserver')
+evaluationDependsOn(':wpimath')
 evaluationDependsOn(':wpilibc')
 evaluationDependsOn(':wpilibj')
 evaluationDependsOn(':wpilibOldCommands')
@@ -30,6 +31,7 @@ cppProjectZips.add(project(':wpiutil').cppHeadersZip)
 cppProjectZips.add(project(':ntcore').cppHeadersZip)
 cppProjectZips.add(project(':cscore').cppHeadersZip)
 cppProjectZips.add(project(':cameraserver').cppHeadersZip)
+cppProjectZips.add(project(':wpimath').cppHeadersZip)
 cppProjectZips.add(project(':wpilibc').cppHeadersZip)
 cppProjectZips.add(project(':wpilibOldCommands').cppHeadersZip)
 cppProjectZips.add(project(':wpilibNewCommands').cppHeadersZip)
@@ -102,18 +104,19 @@ ext {
 apply from: "${rootDir}/shared/opencv.gradle"

 task generateJavaDocs(type: Javadoc) {
-    classpath += project(":wpiutil").sourceSets.main.compileClasspath
+    classpath += project(":wpimath").sourceSets.main.compileClasspath
    options.links("https://docs.oracle.com/en/java/javase/11/docs/api/")
    options.addStringOption "tag", "pre:a:Pre-Condition"
    options.addBooleanOption "Xdoclint:html,missing,reference,syntax", true
    options.addBooleanOption('html5', true)
    dependsOn project(':wpilibj').generateJavaVersion
    dependsOn project(':hal').generateUsageReporting
-    dependsOn project(':wpiutil').generateNat
+    dependsOn project(':wpimath').generateNat
    source project(':hal').sourceSets.main.java
    source project(':wpiutil').sourceSets.main.java
    source project(':cscore').sourceSets.main.java
    source project(':ntcore').sourceSets.main.java
+    source project(':wpimath').sourceSets.main.java
    source project(':wpilibj').sourceSets.main.java
    source project(':cameraserver').sourceSets.main.java
    source project(':wpilibOldCommands').sourceSets.main.java
--- a/myRobot/build.gradle
+++ b/myRobot/build.gradle
@@ -119,6 +119,7 @@ repositories {

 dependencies {
    implementation project(':wpilibj')
+    implementation project(':wpimath')
    implementation project(':hal')
    implementation project(':wpiutil')
    implementation project(':ntcore')
@@ -148,6 +149,7 @@ model {
                    lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
                    lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
                    lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+                    lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                    lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                    lib project: ':cscore', library: 'cscore', linkage: 'shared'
                    lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
@@ -180,6 +182,7 @@ model {
                    lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'static'
                    lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'static'
                    lib project: ':wpilibc', library: 'wpilibc', linkage: 'static'
+                    lib project: ':wpimath', library: 'wpimath', linkage: 'static'
                    lib project: ':ntcore', library: 'ntcore', linkage: 'static'
                    lib project: ':cscore', library: 'cscore', linkage: 'static'
                    project(':hal').addHalDependency(binary, 'static')
--- a/settings.gradle
+++ b/settings.gradle
@@ -20,6 +20,7 @@ include 'wpiutil'
 include 'ntcore'
 include 'hal'
 include 'cscore'
+include 'wpimath'
 include 'wpilibc'
 include 'wpilibcExamples'
 include 'wpilibcIntegrationTests'
--- a/simulation/halsim_gui/CMakeLists.txt
+++ b/simulation/halsim_gui/CMakeLists.txt
@@ -7,7 +7,7 @@ file(GLOB halsim_gui_src src/main/native/cpp/*.cpp)
 add_library(halsim_gui MODULE ${halsim_gui_src})
 wpilib_target_warnings(halsim_gui)
 set_target_properties(halsim_gui PROPERTIES DEBUG_POSTFIX "d")
-target_link_libraries(halsim_gui PUBLIC hal ntcore PRIVATE imgui)
+target_link_libraries(halsim_gui PUBLIC hal ntcore wpimath PRIVATE imgui)

 target_include_directories(halsim_gui PRIVATE src/main/native/include)

--- a/simulation/halsim_gui/build.gradle
+++ b/simulation/halsim_gui/build.gradle
@@ -23,6 +23,7 @@ if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxra
    model {
        binaries {
            all {
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                nativeUtils.useRequiredLibrary(it, 'imgui_static')
                if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
                    it.buildable = false
--- a/wpilibNewCommands/build.gradle
+++ b/wpilibNewCommands/build.gradle
@@ -6,6 +6,7 @@ ext {
 evaluationDependsOn(':ntcore')
 evaluationDependsOn(':cscore')
 evaluationDependsOn(':hal')
+evaluationDependsOn(':wpimath')
 evaluationDependsOn(':wpilibc')
 evaluationDependsOn(':cameraserver')
 evaluationDependsOn(':wpilibj')
@@ -17,11 +18,13 @@ dependencies {
    implementation project(':ntcore')
    implementation project(':cscore')
    implementation project(':hal')
+    implementation project(':wpimath')
    implementation project(':wpilibj')
    devImplementation project(':wpiutil')
    devImplementation project(':ntcore')
    devImplementation project(':cscore')
    devImplementation project(':hal')
+    devImplementation project(':wpimath')
    devImplementation project(':wpilibj')
    testImplementation 'com.google.guava:guava:19.0'
    testImplementation 'org.mockito:mockito-core:2.27.0'
@@ -53,6 +56,7 @@ model {
            lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
            project(':hal').addHalDependency(it, 'shared')
            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            lib project: ':wpimath', library: 'wpimath', linkage: 'shared'

            if (it.component.name == "${nativeName}Dev") {
              lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
--- a/wpilibOldCommands/build.gradle
+++ b/wpilibOldCommands/build.gradle
@@ -6,6 +6,7 @@ ext {
 evaluationDependsOn(':ntcore')
 evaluationDependsOn(':cscore')
 evaluationDependsOn(':hal')
+evaluationDependsOn(':wpimath')
 evaluationDependsOn(':wpilibc')
 evaluationDependsOn(':cameraserver')
 evaluationDependsOn(':wpilibj')
@@ -17,11 +18,13 @@ dependencies {
    implementation project(':ntcore')
    implementation project(':cscore')
    implementation project(':hal')
+    implementation project(':wpimath')
    implementation project(':wpilibj')
    devImplementation project(':wpiutil')
    devImplementation project(':ntcore')
    devImplementation project(':cscore')
    devImplementation project(':hal')
+    devImplementation project(':wpimath')
    devImplementation project(':wpilibj')
 }

@@ -51,6 +54,7 @@ model {
            lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
            project(':hal').addHalDependency(it, 'shared')
            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            lib project: ':wpimath', library: 'wpimath', linkage: 'shared'

            if (it.component.name == "${nativeName}Dev") {
              lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
--- a/wpilibc/CMakeLists.txt
+++ b/wpilibc/CMakeLists.txt
@@ -17,7 +17,7 @@ target_include_directories(wpilibc PUBLIC
                $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
                            $<INSTALL_INTERFACE:${include_dest}/wpilibc>)
 wpilib_target_warnings(wpilibc)
-target_link_libraries(wpilibc PUBLIC cameraserver hal ntcore cscore wpiutil ${OpenCV_LIBS})
+target_link_libraries(wpilibc PUBLIC cameraserver hal ntcore cscore wpimath wpiutil ${OpenCV_LIBS})

 set_property(TARGET wpilibc PROPERTY FOLDER "libraries")

--- a/wpilibc/build.gradle
+++ b/wpilibc/build.gradle
@@ -98,6 +98,7 @@ model {
                lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                project(':hal').addHalDependency(it, 'shared')
                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
            }
        }
        "${nativeName}"(NativeLibrarySpec) {
@@ -116,6 +117,7 @@ model {
                lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                project(':hal').addHalDependency(it, 'shared')
                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'

                if (it instanceof SharedLibraryBinarySpec) {
                    cppCompiler.define 'DYNAMIC_CAMERA_SERVER'
@@ -152,6 +154,7 @@ model {
                lib project: ':cscore', library: 'cscore', linkage: 'shared'
                project(':hal').addHalDependency(it, 'shared')
                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                nativeUtils.useRequiredLibrary(it, 'opencv_shared')
                if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
@@ -201,6 +204,7 @@ model {
            lib project: ':cscore', library: 'cscore', linkage: 'shared'
            project(':hal').addHalDependency(it, 'shared')
            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
            lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
            nativeUtils.useRequiredLibrary(it, 'opencv_shared')
            lib library: nativeName, linkage: 'shared'
--- a/wpilibc/src/main/native/cppcs/RobotBase.cpp
+++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -16,6 +16,7 @@
 #include <cameraserver/CameraServerShared.h>
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
+#include <math/MathShared.h>
 #include <networktables/NetworkTableInstance.h>

 #include "WPILibVersion.h"
@@ -25,7 +26,6 @@
 #include "frc/WPIErrors.h"
 #include "frc/livewindow/LiveWindow.h"
 #include "frc/smartdashboard/SmartDashboard.h"
-#include "frc/trajectory/TrajectoryGenerator.h"

 typedef void (*SetCameraServerSharedFP)(frc::CameraServerShared* shared);

@@ -69,6 +69,47 @@ class WPILibCameraServerShared : public frc::CameraServerShared {
    return std::make_pair(RobotBase::GetThreadId(), true);
  }
 };
+class WPILibMathShared : public wpi::math::MathShared {
+ public:
+  void ReportError(const wpi::Twine& error) override {
+    DriverStation::ReportError(error);
+  }
+
+  void ReportUsage(wpi::math::MathUsageId id, int count) override {
+    switch (id) {
+      case wpi::math::MathUsageId::kKinematics_DifferentialDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+                   HALUsageReporting::kKinematics_DifferentialDrive);
+        break;
+      case wpi::math::MathUsageId::kKinematics_MecanumDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+                   HALUsageReporting::kKinematics_MecanumDrive);
+        break;
+      case wpi::math::MathUsageId::kKinematics_SwerveDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+                   HALUsageReporting::kKinematics_SwerveDrive);
+        break;
+      case wpi::math::MathUsageId::kTrajectory_TrapezoidProfile:
+        HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, count);
+        break;
+      case wpi::math::MathUsageId::kFilter_Linear:
+        HAL_Report(HALUsageReporting::kResourceType_LinearFilter, count);
+        break;
+      case wpi::math::MathUsageId::kOdometry_DifferentialDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Odometry,
+                   HALUsageReporting::kOdometry_DifferentialDrive);
+        break;
+      case wpi::math::MathUsageId::kOdometry_SwerveDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Odometry,
+                   HALUsageReporting::kOdometry_SwerveDrive);
+        break;
+      case wpi::math::MathUsageId::kOdometry_MecanumDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Odometry,
+                   HALUsageReporting::kOdometry_MecanumDrive);
+        break;
+    }
+  }
+};
 }  // namespace

 static void SetupCameraServerShared() {
@@ -102,6 +143,11 @@ static void SetupCameraServerShared() {
 #endif
 }

+static void SetupMathShared() {
+  wpi::math::MathSharedStore::SetMathShared(
+      std::make_unique<WPILibMathShared>());
+}
+
 bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }

 bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); }
@@ -120,8 +166,7 @@ RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
  m_threadId = std::this_thread::get_id();

  SetupCameraServerShared();
-  TrajectoryGenerator::SetErrorHandler(
-      [](const char* error) { DriverStation::ReportError(error); });
+  SetupMathShared();

  auto inst = nt::NetworkTableInstance::GetDefault();
  inst.SetNetworkIdentity("Robot");
--- a/wpilibcExamples/build.gradle
+++ b/wpilibcExamples/build.gradle
@@ -57,6 +57,7 @@ model {
                lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
                lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
                lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                lib project: ':cscore', library: 'cscore', linkage: 'shared'
                project(':hal').addHalDependency(binary, 'shared')
@@ -84,6 +85,7 @@ model {
                    lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
                    lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
                    lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+                    lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                    lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                    lib project: ':cscore', library: 'cscore', linkage: 'shared'
                    project(':hal').addHalDependency(binary, 'shared')
@@ -126,6 +128,7 @@ model {
                    lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
                    lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
                    lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+                    lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                    lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                    lib project: ':cscore', library: 'cscore', linkage: 'shared'
                    project(':hal').addHalDependency(binary, 'shared')
--- a/wpilibcIntegrationTests/build.gradle
+++ b/wpilibcIntegrationTests/build.gradle
@@ -46,6 +46,7 @@ model {
                    }
                    lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
                    lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+                    lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                    lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                    lib project: ':cscore', library: 'cscore', linkage: 'shared'
                    lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
--- a/wpilibj/CMakeLists.txt
+++ b/wpilibj/CMakeLists.txt
@@ -15,10 +15,10 @@ if (NOT WITHOUT_JAVA)
    configure_file(src/generate/WPILibVersion.java.in WPILibVersion.java)

    file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
-    file(GLOB EJML_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml/*.jar")
+    file(GLOB EJML_JARS "${CMAKE_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
    file(GLOB JACKSON_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")

-    add_jar(wpilibj_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java INCLUDE_JARS hal_jar ntcore_jar ${EJML_JARS} ${JACKSON_JARS} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpiutil_jar OUTPUT_NAME wpilibj)
+    add_jar(wpilibj_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java INCLUDE_JARS hal_jar ntcore_jar ${EJML_JARS} ${JACKSON_JARS} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpimath_jar wpiutil_jar OUTPUT_NAME wpilibj)

    get_property(WPILIBJ_JAR_FILE TARGET wpilibj_jar PROPERTY JAR_FILE)
    install(FILES ${WPILIBJ_JAR_FILE} DESTINATION "${java_lib_dest}")
--- a/wpilibj/build.gradle
+++ b/wpilibj/build.gradle
@@ -62,6 +62,7 @@ repositories {
 dependencies {
    implementation project(':hal')
    implementation project(':wpiutil')
+    implementation project(':wpimath')
    implementation project(':ntcore')
    implementation project(':cscore')
    implementation project(':cameraserver')
@@ -69,6 +70,7 @@ dependencies {
    testImplementation 'org.mockito:mockito-core:2.27.0'
    devImplementation project(':hal')
    devImplementation project(':wpiutil')
+    devImplementation project(':wpimath')
    devImplementation project(':ntcore')
    devImplementation project(':cscore')
    devImplementation project(':cameraserver')
@@ -110,9 +112,11 @@ model {
                lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                lib project: ':cscore', library: 'cscore', linkage: 'shared'
                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
                lib project: ':cscore', library: 'cscoreJNIShared', linkage: 'shared'
                lib project: ':wpiutil', library: 'wpiutilJNIShared', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimathJNIShared', linkage: 'shared'
                lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                project(':hal').addHalDependency(it, 'shared')
                project(':hal').addHalJniDependency(it)
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.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.                                                               */
@@ -22,6 +22,9 @@ import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.math.MathShared;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -78,6 +81,53 @@ public abstract class RobotBase implements AutoCloseable {
    CameraServerSharedStore.setCameraServerShared(shared);
  }

+  private static void setupMathShared() {
+    MathSharedStore.setMathShared(new MathShared() {
+      @Override
+      public void reportError(String error, StackTraceElement[] stackTrace) {
+        DriverStation.reportError(error, stackTrace);
+      }
+
+      @Override
+      public void reportUsage(MathUsageId id, int count) {
+        switch (id) {
+          case kKinematics_DifferentialDrive:
+            HAL.report(tResourceType.kResourceType_Kinematics,
+                tInstances.kKinematics_DifferentialDrive);
+            break;
+          case kKinematics_MecanumDrive:
+            HAL.report(tResourceType.kResourceType_Kinematics,
+                tInstances.kKinematics_MecanumDrive);
+            break;
+          case kKinematics_SwerveDrive:
+            HAL.report(tResourceType.kResourceType_Kinematics,
+                tInstances.kKinematics_SwerveDrive);
+            break;
+          case kTrajectory_TrapezoidProfile:
+            HAL.report(tResourceType.kResourceType_TrapezoidProfile, count);
+            break;
+          case kFilter_Linear:
+            HAL.report(tResourceType.kResourceType_LinearFilter, count);
+            break;
+          case kOdometry_DifferentialDrive:
+            HAL.report(tResourceType.kResourceType_Odometry,
+                tInstances.kOdometry_DifferentialDrive);
+            break;
+          case kOdometry_SwerveDrive:
+            HAL.report(tResourceType.kResourceType_Odometry,
+                tInstances.kOdometry_SwerveDrive);
+            break;
+          case kOdometry_MecanumDrive:
+            HAL.report(tResourceType.kResourceType_Odometry,
+                tInstances.kOdometry_MecanumDrive);
+            break;
+          default:
+            break;
+        }
+      }
+    });
+  }
+
  protected final DriverStation m_ds;

  /**
@@ -90,9 +140,10 @@ public abstract class RobotBase implements AutoCloseable {
   * to put this code into it's own task that loads on boot so ensure that it runs.
   */
  protected RobotBase() {
-    NetworkTableInstance inst = NetworkTableInstance.getDefault();
+    final NetworkTableInstance inst = NetworkTableInstance.getDefault();
    m_threadId = Thread.currentThread().getId();
    setupCameraServerShared();
+    setupMathShared();
    inst.setNetworkIdentity("Robot");
    if (isReal()) {
      inst.startServer("/home/lvuser/networktables.ini");
--- a/wpilibjExamples/build.gradle
+++ b/wpilibjExamples/build.gradle
@@ -13,6 +13,7 @@ apply from: "${rootDir}/shared/opencv.gradle"
 dependencies {
    implementation project(':wpilibj')

+    implementation project(':wpimath')
    implementation project(':hal')
    implementation project(':wpiutil')
    implementation project(':ntcore')
--- a/wpilibjIntegrationTests/build.gradle
+++ b/wpilibjIntegrationTests/build.gradle
@@ -21,6 +21,7 @@ repositories {

 dependencies {
    implementation project(':wpilibj')
+    implementation project(':wpimath')
    implementation project(':hal')
    implementation project(':wpiutil')
    implementation project(':ntcore')
--- a/wpimath/.styleguide
+++ b/wpimath/.styleguide
@@ -0,0 +1,37 @@
+cppHeaderFileInclude {
+  \.h$
+  \.inc$
+  \.inl$
+}
+
+cppSrcFileInclude {
+  \.cpp$
+}
+
+generatedFileExclude {
+  src/main/native/cpp/drake/
+  src/main/native/include/Eigen/
+  src/main/native/include/drake/
+  src/main/native/include/units/base\.h$
+  src/main/native/include/units/units\.h$
+  src/main/native/include/unsupported/
+  src/test/native/cpp/UnitsTest\.cpp$
+  src/test/native/cpp/drake/
+  src/test/native/include/drake/
+}
+
+repoRootNameOverride {
+  wpimath
+}
+
+includeGuardRoots {
+  wpimath/src/main/native/cpp/
+  wpimath/src/main/native/include/
+  wpimath/src/test/native/cpp/
+}
+
+includeOtherLibs {
+  ^Eigen/
+  ^unsupported/
+  ^wpi/
+}
--- a/wpimath/CMakeLists.txt
+++ b/wpimath/CMakeLists.txt
@@ -0,0 +1,142 @@
+project(wpimath)
+
+include(SubDirList)
+include(CompileWarnings)
+include(AddTest)
+
+file(GLOB wpimath_jni_src src/main/native/cpp/jni/DrakeJNI.cpp)
+
+# Java bindings
+if (NOT WITHOUT_JAVA)
+  find_package(Java REQUIRED)
+  find_package(JNI REQUIRED)
+  include(UseJava)
+  set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
+
+  if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.38.jar")
+      set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
+      set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpimath/thirdparty/ejml")
+
+      message(STATUS "Downloading EJML jarfiles...")
+
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-cdense/0.38/ejml-cdense-0.38.jar"
+          "${JAR_ROOT}/ejml-cdense-0.38.jar")
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-core/0.38/ejml-core-0.38.jar"
+          "${JAR_ROOT}/ejml-core-0.38.jar")
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-ddense/0.38/ejml-ddense-0.38.jar"
+          "${JAR_ROOT}/ejml-ddense-0.38.jar")
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-dsparse/0.38/ejml-dsparse-0.38.jar"
+          "${JAR_ROOT}/ejml-dsparse-0.38.jar")
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-fdense/0.38/ejml-fdense-0.38.jar"
+          "${JAR_ROOT}/ejml-fdense-0.38.jar")
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-simple/0.38/ejml-simple-0.38.jar"
+          "${JAR_ROOT}/ejml-simple-0.38.jar")
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-zdense/0.38/ejml-zdense-0.38.jar"
+          "${JAR_ROOT}/ejml-zdense-0.38.jar")
+
+      message(STATUS "All files downloaded.")
+  endif()
+
+  file(GLOB EJML_JARS "${CMAKE_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
+  file(GLOB JACKSON_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
+
+  set(CMAKE_JAVA_INCLUDE_PATH wpimath.jar ${EJML_JARS} ${JACKSON_JARS})
+
+  execute_process(COMMAND python3 ${CMAKE_SOURCE_DIR}/wpimath/generate_numbers.py ${CMAKE_BINARY_DIR}/wpimath RESULT_VARIABLE generateResult)
+  if(NOT (generateResult EQUAL "0"))
+    # Try python
+    execute_process(COMMAND python ${CMAKE_SOURCE_DIR}/wpimath/generate_numbers.py ${CMAKE_BINARY_DIR}/wpimath RESULT_VARIABLE generateResult)
+    if(NOT (generateResult EQUAL "0"))
+      message(FATAL_ERROR "python and python3 generate_numbers.py failed")
+    endif()
+  endif()
+
+  set(CMAKE_JNI_TARGET true)
+
+  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java ${CMAKE_BINARY_DIR}/wpimath/generated/*.java)
+
+  if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
+    set(CMAKE_JAVA_COMPILE_FLAGS "-h" "${CMAKE_CURRENT_BINARY_DIR}/jniheaders")
+    add_jar(wpimath_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} wpiutil_jar OUTPUT_NAME wpimath)
+  else()
+    add_jar(wpimath_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} wpiutil_jar OUTPUT_NAME wpimath GENERATE_NATIVE_HEADERS wpimath_jni_headers)
+  endif()
+
+  get_property(WPIMATH_JAR_FILE TARGET wpimath_jar PROPERTY JAR_FILE)
+  install(FILES ${WPIMATH_JAR_FILE} DESTINATION "${java_lib_dest}")
+
+  set_property(TARGET wpimath_jar PROPERTY FOLDER "java")
+
+  add_library(wpimathjni ${wpimath_jni_src})
+  wpilib_target_warnings(wpimathjni)
+  target_link_libraries(wpimathjni PUBLIC wpimath)
+
+  set_property(TARGET wpimathjni PROPERTY FOLDER "libraries")
+
+  if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
+    target_include_directories(wpimathjni PRIVATE ${JNI_INCLUDE_DIRS})
+    target_include_directories(wpimathjni PRIVATE "${CMAKE_CURRENT_BINARY_DIR}/jniheaders")
+  else()
+    target_link_libraries(wpimathjni PRIVATE wpimath_jni_headers)
+  endif()
+  add_dependencies(wpimathjni wpimath_jar)
+
+  if (MSVC)
+    install(TARGETS wpimathjni RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
+  endif()
+
+  install(TARGETS wpimathjni EXPORT wpimathjni DESTINATION "${main_lib_dest}")
+
+endif()
+
+file(GLOB_RECURSE wpimath_native_src src/main/native/cpp/*.cpp)
+list(REMOVE_ITEM wpimath_native_src ${wpimath_jni_src})
+
+add_library(wpimath ${wpimath_native_src})
+set_target_properties(wpimath PROPERTIES DEBUG_POSTFIX "d")
+
+set_property(TARGET wpimath PROPERTY FOLDER "libraries")
+
+target_compile_features(wpimath PUBLIC cxx_std_17)
+if (MSVC)
+    target_compile_options(wpimath PUBLIC /bigobj)
+endif()
+wpilib_target_warnings(wpimath)
+target_link_libraries(wpimath wpiutil)
+
+if (NOT USE_VCPKG_EIGEN)
+    install(DIRECTORY src/main/native/eigeninclude/ DESTINATION "${include_dest}/wpimath")
+    target_include_directories(wpimath PUBLIC
+                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/eigeninclude>
+                            $<INSTALL_INTERFACE:${include_dest}/wpimath>)
+else()
+    find_package(Eigen3 CONFIG REQUIRED)
+    target_link_libraries (wpimath Eigen3::Eigen)
+endif()
+
+target_include_directories(wpimath PUBLIC
+                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+                            $<INSTALL_INTERFACE:${include_dest}/wpimath>)
+
+install(TARGETS wpimath EXPORT wpimath DESTINATION "${main_lib_dest}")
+install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpimath")
+
+if (NOT WITHOUT_JAVA AND MSVC)
+    install(TARGETS wpimath RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
+endif()
+
+if (MSVC OR FLAT_INSTALL_WPILIB)
+    set (wpimath_config_dir ${wpilib_dest})
+else()
+    set (wpimath_config_dir share/wpimath)
+endif()
+
+configure_file(wpimath-config.cmake.in ${CMAKE_BINARY_DIR}/wpimath-config.cmake )
+install(FILES ${CMAKE_BINARY_DIR}/wpimath-config.cmake DESTINATION ${wpimath_config_dir})
+install(EXPORT wpimath DESTINATION ${wpimath_config_dir})
+
+if (WITH_TESTS)
+    wpilib_add_test(wpimath src/test/native/cpp)
+    target_include_directories(wpimath_test PRIVATE src/test/native/include)
+    target_link_libraries(wpimath_test wpimath gmock_main)
+endif()
--- a/wpimath/build.gradle
+++ b/wpimath/build.gradle
@@ -0,0 +1,103 @@
+ext {
+    useJava = true
+    useCpp = true
+    baseId = 'wpimath'
+    groupId = 'edu.wpi.first.wpimath'
+
+    nativeName = 'wpimath'
+    devMain = 'edu.wpi.first.wpiutil.math.DevMain'
+}
+
+apply from: "${rootDir}/shared/jni/setupBuild.gradle"
+
+nativeUtils.exportsConfigs {
+    wpimath {
+        x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
+                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
+                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
+                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+        x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
+                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
+                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
+                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+    }
+}
+
+model {
+    components {
+        all {
+            it.sources.each {
+                it.exportedHeaders {
+                    srcDirs 'src/main/native/include'
+                }
+            }
+        }
+    }
+}
+
+dependencies {
+    api "org.ejml:ejml-simple:0.38"
+    api "com.fasterxml.jackson.core:jackson-annotations:2.10.0"
+    api "com.fasterxml.jackson.core:jackson-core:2.10.0"
+    api "com.fasterxml.jackson.core:jackson-databind:2.10.0"
+}
+
+def wpilibNumberFileInput = file("src/generate/GenericNumber.java.in")
+def natFileInput = file("src/generate/Nat.java.in")
+def natGetterInput = file("src/generate/NatGetter.java.in")
+def wpilibNumberFileOutputDir = file("$buildDir/generated/java/edu/wpi/first/wpiutil/math/numbers")
+def wpilibNatFileOutput = file("$buildDir/generated/java/edu/wpi/first/wpiutil/math/Nat.java")
+def maxNum = 20
+
+task generateNumbers() {
+    description = "Generates generic number classes from template"
+    group = "WPILib"
+
+    inputs.file wpilibNumberFileInput
+    outputs.dir wpilibNumberFileOutputDir
+
+    doLast {
+        if(wpilibNumberFileOutputDir.exists()) {
+            wpilibNumberFileOutputDir.delete()
+        }
+        wpilibNumberFileOutputDir.mkdirs()
+
+        for(i in 0..maxNum) {
+            def outputFile = new File(wpilibNumberFileOutputDir, "N${i}.java")
+            def read = wpilibNumberFileInput.text.replace('${num}', i.toString())
+            outputFile.write(read)
+        }
+    }
+}
+
+task generateNat() {
+    description = "Generates Nat.java"
+    group = "WPILib"
+    inputs.files([natFileInput, natGetterInput])
+    outputs.file wpilibNatFileOutput
+    dependsOn generateNumbers
+
+    doLast {
+        if(wpilibNatFileOutput.exists()) {
+            wpilibNatFileOutput.delete()
+        }
+
+        def template = natFileInput.text + "\n"
+
+        def importsString = "";
+
+        for(i in 0..maxNum) {
+            importsString += "import edu.wpi.first.wpiutil.math.numbers.N${i};\n"
+            template += natGetterInput.text.replace('${num}', i.toString()) + "\n"
+        }
+        template += "}\n" // Close the class body
+
+        template = template.replace('{{REPLACEWITHIMPORTS}}', importsString)
+
+        wpilibNatFileOutput.write(template)
+    }
+}
+
+sourceSets.main.java.srcDir "${buildDir}/generated/java"
+compileJava.dependsOn generateNumbers
+compileJava.dependsOn generateNat
--- a/wpimath/generate_numbers.py
+++ b/wpimath/generate_numbers.py
--- a/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java
+++ b/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+public final class DevMain {
+  /**
+   * Main entry point.
+   */
+  public static void main(String[] args) {
+    System.out.println("Hello World!");
+    System.out.println(MathUtil.normalizeAngle(-5.0));
+  }
+
+  private DevMain() {
+  }
+}
--- a/wpimath/src/dev/native/cpp/main.cpp
+++ b/wpimath/src/dev/native/cpp/main.cpp
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <iostream>
+
+#include <wpi/math>
+
+int main() { std::cout << wpi::math::pi << std::endl; }
--- a/wpimath/src/generate/GenericNumber.java.in
+++ b/wpimath/src/generate/GenericNumber.java.in
--- a/wpimath/src/generate/Nat.java.in
+++ b/wpimath/src/generate/Nat.java.in
--- a/wpimath/src/generate/NatGetter.java.in
+++ b/wpimath/src/generate/NatGetter.java.in
--- a/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+public interface MathShared {
+  /**
+   * Report an error.
+   *
+   * @param error the error to set
+   */
+  void reportError(String error, StackTraceElement[] stackTrace);
+
+  /**
+   * Report usage.
+   *
+   * @param id the usage id
+   * @param count the usage count
+   */
+  void reportUsage(MathUsageId id, int count);
+}
--- a/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+public final class MathSharedStore {
+  private static MathShared mathShared;
+
+  private MathSharedStore() {
+  }
+
+  /**
+   * get the MathShared object.
+   */
+  public static synchronized MathShared getMathShared() {
+    if (mathShared == null) {
+      mathShared = new MathShared() {
+        @Override
+        public void reportError(String error, StackTraceElement[] stackTrace) {
+        }
+
+        @Override
+        public void reportUsage(MathUsageId id, int count) {
+        }
+      };
+    }
+    return mathShared;
+  }
+
+  /**
+   * set the MathShared object.
+   */
+  public static synchronized void setMathShared(MathShared shared) {
+    mathShared = shared;
+  }
+
+  /**
+   * Report an error.
+   *
+   * @param error the error to set
+   */
+  public static void reportError(String error, StackTraceElement[] stackTrace) {
+    getMathShared().reportError(error, stackTrace);
+  }
+
+  /**
+   * Report usage.
+   *
+   * @param id the usage id
+   * @param count the usage count
+   */
+  public static void reportUsage(MathUsageId id, int count) {
+    getMathShared().reportUsage(id, count);
+  }
+}
--- a/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+public enum MathUsageId {
+  kKinematics_DifferentialDrive,
+  kKinematics_MecanumDrive,
+  kKinematics_SwerveDrive,
+  kTrajectory_TrapezoidProfile,
+  kFilter_Linear,
+  kOdometry_DifferentialDrive,
+  kOdometry_SwerveDrive,
+  kOdometry_MecanumDrive
+}
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
@@ -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.                                                               */
@@ -9,8 +9,8 @@ package edu.wpi.first.wpilibj;

 import java.util.Arrays;

-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.wpiutil.CircularBuffer;

 /**
@@ -71,7 +71,7 @@ public class LinearFilter {
    m_outputGains = Arrays.copyOf(fbGains, fbGains.length);

    instances++;
-    HAL.report(tResourceType.kResourceType_LinearFilter, instances);
+    MathSharedStore.reportUsage(MathUsageId.kFilter_Linear, instances);
  }

  /**
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/MedianFilter.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.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.                                                               */
@@ -7,9 +7,8 @@

 package edu.wpi.first.wpilibj.kinematics;

-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;

 /**
 * Helper class that converts a chassis velocity (dx and dtheta components) to
@@ -33,7 +32,7 @@ public class DifferentialDriveKinematics {
   */
  public DifferentialDriveKinematics(double trackWidthMeters) {
    this.trackWidthMeters = trackWidthMeters;
-    HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_DifferentialDrive);
+    MathSharedStore.reportUsage(MathUsageId.kKinematics_DifferentialDrive, 1);
  }

  /**
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.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.                                                               */
@@ -7,9 +7,8 @@

 package edu.wpi.first.wpilibj.kinematics;

-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Twist2d;
@@ -46,7 +45,7 @@ public class DifferentialDriveOdometry {
    m_poseMeters = initialPoseMeters;
    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
    m_previousAngle = initialPoseMeters.getRotation();
-    HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_DifferentialDrive);
+    MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
  }

  /**
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.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.                                                               */
@@ -9,9 +9,8 @@ package edu.wpi.first.wpilibj.kinematics;

 import org.ejml.simple.SimpleMatrix;

-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.wpilibj.geometry.Translation2d;

 /**
@@ -74,7 +73,7 @@ public class MecanumDriveKinematics {
        rearLeftWheelMeters, rearRightWheelMeters);
    m_forwardKinematics = m_inverseKinematics.pseudoInverse();

-    HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_MecanumDrive);
+    MathSharedStore.reportUsage(MathUsageId.kKinematics_MecanumDrive, 1);
  }

  /**
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.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.                                                               */
@@ -7,13 +7,12 @@

 package edu.wpi.first.wpilibj.kinematics;

-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Twist2d;
+import edu.wpi.first.wpiutil.WPIUtilJNI;

 /**
 * Class for mecanum drive odometry. Odometry allows you to track the robot's
@@ -45,7 +44,7 @@ public class MecanumDriveOdometry {
    m_poseMeters = initialPoseMeters;
    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
    m_previousAngle = initialPoseMeters.getRotation();
-    HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_MecanumDrive);
+    MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
  }

  /**
@@ -127,7 +126,7 @@ public class MecanumDriveOdometry {
   */
  public Pose2d update(Rotation2d gyroAngle,
                       MecanumDriveWheelSpeeds wheelSpeeds) {
-    return updateWithTime(Timer.getFPGATimestamp(), gyroAngle,
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle,
        wheelSpeeds);
  }
 }
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.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.                                                               */
@@ -12,9 +12,8 @@ import java.util.Collections;

 import org.ejml.simple.SimpleMatrix;

-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Translation2d;

@@ -73,7 +72,7 @@ public class SwerveDriveKinematics {
    }
    m_forwardKinematics = m_inverseKinematics.pseudoInverse();

-    HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_SwerveDrive);
+    MathSharedStore.reportUsage(MathUsageId.kKinematics_SwerveDrive, 1);
  }

  /**
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.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.                                                               */
@@ -7,13 +7,12 @@

 package edu.wpi.first.wpilibj.kinematics;

-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Twist2d;
+import edu.wpi.first.wpiutil.WPIUtilJNI;

 /**
 * Class for swerve drive odometry. Odometry allows you to track the robot's
@@ -45,7 +44,7 @@ public class SwerveDriveOdometry {
    m_poseMeters = initialPose;
    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
    m_previousAngle = initialPose.getRotation();
-    HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_SwerveDrive);
+    MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
  }

  /**
@@ -131,6 +130,6 @@ public class SwerveDriveOdometry {
   * @return The new pose of the robot.
   */
  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
-    return updateWithTime(Timer.getFPGATimestamp(), gyroAngle, moduleStates);
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
  }
 }
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/Spline.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
@@ -13,7 +13,7 @@ import java.util.Collection;
 import java.util.List;
 import java.util.function.BiConsumer;

-import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Transform2d;
@@ -39,7 +39,7 @@ public final class TrajectoryGenerator {
    if (errorFunc != null) {
      errorFunc.accept(error, stackTrace);
    } else {
-      DriverStation.reportError(error, stackTrace);
+      MathSharedStore.reportError(error, stackTrace);
    }
  }

--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
@@ -9,8 +9,8 @@ package edu.wpi.first.wpilibj.trajectory;

 import java.util.Objects;

-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;

 /**
 * A trapezoid-shaped velocity profile.
@@ -62,7 +62,7 @@ public class TrapezoidProfile {
    public double maxAcceleration;

    public Constraints() {
-      HAL.report(tResourceType.kResourceType_TrapezoidProfile, 1);
+      MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
    }

    /**
@@ -74,7 +74,7 @@ public class TrapezoidProfile {
    public Constraints(double maxVelocity, double maxAcceleration) {
      this.maxVelocity = maxVelocity;
      this.maxAcceleration = maxAcceleration;
-      HAL.report(tResourceType.kResourceType_TrapezoidProfile, 1);
+      MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
    }
  }

--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
@@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;

-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import static edu.wpi.first.wpiutil.ErrorMessages.requireNonNullParam;

 /**
 * A class that enforces constraints on differential drive voltage expenditure based on the motor
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/util/Units.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/util/Units.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Drake.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Drake.java
--- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/DrakeJNI.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/DrakeJNI.java
@@ -19,7 +19,7 @@ public final class DrakeJNI {
  static {
    if (Helper.getExtractOnStaticLoad()) {
      try {
-        loader = new RuntimeLoader<>("wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(),
+        loader = new RuntimeLoader<>("wpimathjni", RuntimeLoader.getDefaultExtractionRoot(),
                DrakeJNI.class);
        loader.loadLibrary();
      } catch (IOException ex) {
@@ -39,7 +39,7 @@ public final class DrakeJNI {
    if (libraryLoaded) {
      return;
    }
-    loader = new RuntimeLoader<>("wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(),
+    loader = new RuntimeLoader<>("wpimathjni", RuntimeLoader.getDefaultExtractionRoot(),
            DrakeJNI.class);
    loader.loadLibrary();
    libraryLoaded = true;
--- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
--- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Num.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Num.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.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.                                                               */
--- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.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.                                                               */
--- a/wpimath/src/main/native/cpp/MathShared.cpp
+++ b/wpimath/src/main/native/cpp/MathShared.cpp
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "math/MathShared.h"
+
+#include <wpi/mutex.h>
+
+using namespace wpi::math;
+
+namespace {
+class DefaultMathShared : public MathShared {
+ public:
+  void ReportError(const wpi::Twine& error) override {}
+  void ReportUsage(MathUsageId id, int count) override {}
+};
+}  // namespace
+
+static std::unique_ptr<MathShared> mathShared;
+static wpi::mutex setLock;
+
+MathShared& MathSharedStore::GetMathShared() {
+  std::scoped_lock lock(setLock);
+  if (!mathShared) mathShared = std::make_unique<DefaultMathShared>();
+  return *mathShared;
+}
+
+void MathSharedStore::SetMathShared(std::unique_ptr<MathShared> shared) {
+  std::scoped_lock lock(setLock);
+  mathShared = std::move(shared);
+}
--- a/wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp
+++ b/wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp
--- a/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
+++ b/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
--- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Pose2d.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.                                                               */
--- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -9,9 +9,10 @@

 #include <cmath>

-#include <units/math.h>
 #include <wpi/json.h>

+#include "units/math.h"
+
 using namespace frc;

 Rotation2d::Rotation2d(units::radian_t value)
--- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
--- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
@@ -7,9 +7,10 @@

 #include "frc/geometry/Translation2d.h"

-#include <units/math.h>
 #include <wpi/json.h>

+#include "units/math.h"
+
 using namespace frc;

 Translation2d::Translation2d(units::meter_t x, units::meter_t y)
--- a/wpimath/src/main/native/cpp/jni/DrakeJNI.cpp
+++ b/wpimath/src/main/native/cpp/jni/DrakeJNI.cpp
@@ -7,14 +7,12 @@

 #include <jni.h>

-#include <iostream>
-
 #include <Eigen/Core>
 #include <unsupported/Eigen/MatrixFunctions>
+#include <wpi/jni_util.h>

 #include "drake/math/discrete_algebraic_riccati_equation.h"
 #include "edu_wpi_first_wpiutil_math_DrakeJNI.h"
-#include "wpi/jni_util.h"

 using namespace wpi::java;

--- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.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,7 @@

 #include "frc/kinematics/DifferentialDriveOdometry.h"

-#include <hal/FRCUsageReporting.h>
+#include "math/MathShared.h"

 using namespace frc;

@@ -16,8 +16,8 @@ DifferentialDriveOdometry::DifferentialDriveOdometry(
    : m_pose(initialPose) {
  m_previousAngle = m_pose.Rotation();
  m_gyroOffset = m_pose.Rotation() - gyroAngle;
-  HAL_Report(HALUsageReporting::kResourceType_Odometry,
-             HALUsageReporting::kOdometry_DifferentialDrive);
+  wpi::math::MathSharedStore::ReportUsage(
+      wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1);
 }

 const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
--- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
@@ -7,7 +7,7 @@

 #include "frc/kinematics/DifferentialDriveWheelSpeeds.h"

-#include <units/math.h>
+#include "units/math.h"

 using namespace frc;

--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.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,7 @@

 #include "frc/kinematics/MecanumDriveOdometry.h"

-#include <hal/FRCUsageReporting.h>
+#include "math/MathShared.h"

 using namespace frc;

@@ -17,8 +17,8 @@ MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
    : m_kinematics(kinematics), m_pose(initialPose) {
  m_previousAngle = m_pose.Rotation();
  m_gyroOffset = m_pose.Rotation() - gyroAngle;
-  HAL_Report(HALUsageReporting::kResourceType_Odometry,
-             HALUsageReporting::kOdometry_MecanumDrive);
+  wpi::math::MathSharedStore::ReportUsage(
+      wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);
 }

 const Pose2d& MecanumDriveOdometry::UpdateWithTime(
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
@@ -11,7 +11,7 @@
 #include <array>
 #include <cmath>

-#include <units/math.h>
+#include "units/math.h"

 using namespace frc;

--- a/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
+++ b/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.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.                                                               */
--- a/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
+++ b/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.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.                                                               */
--- a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
+++ b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
--- a/wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp
+++ b/wpimath/src/main/native/cpp/spline/SplineParameterizer.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.                                                               */
--- a/Show More
+++ b/Show More