diff --git a/.styleguide b/.styleguide index f1b849bb0d..51597b7082 100644 --- 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/ } diff --git a/CMakeLists.txt b/CMakeLists.txt index de16fb3b4c..5d5abafaeb 100644 --- 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) diff --git a/docs/build.gradle b/docs/build.gradle index 6f59d98d42..8952e53cc4 100644 --- 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 diff --git a/myRobot/build.gradle b/myRobot/build.gradle index ffb59f0750..f20ba7960e 100644 --- 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') diff --git a/settings.gradle b/settings.gradle index e636ec79a2..897f4addfb 100644 --- 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' diff --git a/simulation/halsim_gui/CMakeLists.txt b/simulation/halsim_gui/CMakeLists.txt index f1804e458e..6a7b2e368b 100644 --- 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) diff --git a/simulation/halsim_gui/build.gradle b/simulation/halsim_gui/build.gradle index 5e4b9dc96b..7b9850bdea 100644 --- 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 diff --git a/wpilibNewCommands/build.gradle b/wpilibNewCommands/build.gradle index 09f2a9547b..82affa053c 100644 --- 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' diff --git a/wpilibOldCommands/build.gradle b/wpilibOldCommands/build.gradle index e6e061d60f..c8a4cb916e 100644 --- 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' diff --git a/wpilibc/CMakeLists.txt b/wpilibc/CMakeLists.txt index 977200a1f4..c341bf60d0 100644 --- a/wpilibc/CMakeLists.txt +++ b/wpilibc/CMakeLists.txt @@ -17,7 +17,7 @@ target_include_directories(wpilibc PUBLIC $ $) 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") diff --git a/wpilibc/build.gradle b/wpilibc/build.gradle index 4ee57667d6..ec85691c92 100644 --- 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' diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp index d4223aedff..e87792eddb 100644 --- a/wpilibc/src/main/native/cppcs/RobotBase.cpp +++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #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()); +} + 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"); diff --git a/wpilibcExamples/build.gradle b/wpilibcExamples/build.gradle index 0a4bea4639..35f20d4c48 100644 --- 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') diff --git a/wpilibcIntegrationTests/build.gradle b/wpilibcIntegrationTests/build.gradle index ed6d58e9f3..81f827b980 100644 --- 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' diff --git a/wpilibj/CMakeLists.txt b/wpilibj/CMakeLists.txt index 7874f7fdf4..35fa07f149 100644 --- 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}") diff --git a/wpilibj/build.gradle b/wpilibj/build.gradle index b2e22fb39d..df16022c4f 100644 --- 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) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java index e2861f4610..916fc44818 100644 --- 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"); diff --git a/wpilibjExamples/build.gradle b/wpilibjExamples/build.gradle index 2016632531..73185ba353 100644 --- 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') diff --git a/wpilibjIntegrationTests/build.gradle b/wpilibjIntegrationTests/build.gradle index ec5714e6b8..090d30947a 100644 --- 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') diff --git a/wpimath/.styleguide b/wpimath/.styleguide new file mode 100644 index 0000000000..4283e61d28 --- /dev/null +++ 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/ +} diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt new file mode 100644 index 0000000000..5ea819b4d8 --- /dev/null +++ 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 + $ + $) +else() + find_package(Eigen3 CONFIG REQUIRED) + target_link_libraries (wpimath Eigen3::Eigen) +endif() + +target_include_directories(wpimath PUBLIC + $ + $) + +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() diff --git a/wpimath/build.gradle b/wpimath/build.gradle new file mode 100644 index 0000000000..be85ac1d6d --- /dev/null +++ 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 diff --git a/wpiutil/generate_numbers.py b/wpimath/generate_numbers.py similarity index 100% rename from wpiutil/generate_numbers.py rename to wpimath/generate_numbers.py diff --git a/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java b/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java new file mode 100644 index 0000000000..9a5d378ba3 --- /dev/null +++ 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() { + } +} diff --git a/wpimath/src/dev/native/cpp/main.cpp b/wpimath/src/dev/native/cpp/main.cpp new file mode 100644 index 0000000000..030c8f9f46 --- /dev/null +++ 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 + +#include + +int main() { std::cout << wpi::math::pi << std::endl; } diff --git a/wpiutil/src/generate/GenericNumber.java.in b/wpimath/src/generate/GenericNumber.java.in similarity index 100% rename from wpiutil/src/generate/GenericNumber.java.in rename to wpimath/src/generate/GenericNumber.java.in diff --git a/wpiutil/src/generate/Nat.java.in b/wpimath/src/generate/Nat.java.in similarity index 100% rename from wpiutil/src/generate/Nat.java.in rename to wpimath/src/generate/Nat.java.in diff --git a/wpiutil/src/generate/NatGetter.java.in b/wpimath/src/generate/NatGetter.java.in similarity index 100% rename from wpiutil/src/generate/NatGetter.java.in rename to wpimath/src/generate/NatGetter.java.in diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathShared.java b/wpimath/src/main/java/edu/wpi/first/math/MathShared.java new file mode 100644 index 0000000000..168dbb5ced --- /dev/null +++ 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); +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java b/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java new file mode 100644 index 0000000000..a4c8425c83 --- /dev/null +++ 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); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java b/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java new file mode 100644 index 0000000000..ed95e2467e --- /dev/null +++ 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 +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java similarity index 96% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java index 94bcb3127f..10897e8242 100644 --- a/wpilibj/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); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java similarity index 97% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java index a4a3a95372..18998b0254 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java index 33f9784a30..59e927a16e 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java index 86a4f7e481..0b52c14d03 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java index 9d2c57ed39..ec53d46a8b 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java similarity index 100% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java similarity index 100% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java similarity index 100% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java similarity index 100% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java similarity index 96% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java index f2def815e0..1482902b91 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java index aa98a477c0..a6878b3f53 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java similarity index 90% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java index bb632af48d..309f5315f6 100644 --- a/wpilibj/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); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java similarity index 93% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java index 35a4342387..86470f8c1f 100644 --- a/wpilibj/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); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java similarity index 97% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java index ac4a9fa0d9..1716430792 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java similarity index 96% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java index a5e5b5ff72..fd3b4ec187 100644 --- a/wpilibj/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); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java similarity index 97% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java index f467d2033a..756bb6042b 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java similarity index 93% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java index b3dd4028a8..cd84bdf736 100644 --- a/wpilibj/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); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java index 0f0dd7113b..f00e4094a2 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java similarity index 96% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java index 37f97b8217..657b39b764 100644 --- a/wpilibj/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); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java similarity index 93% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java index 811c3de435..5b1f975f17 100644 --- a/wpilibj/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); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java similarity index 96% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java index e3119e5fd0..f9570eb5fb 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java index 578787c4f3..f387fc0630 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java similarity index 94% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java index 62c1e5a6c9..ed8562db94 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java index 3b2d3ebebd..6073f62577 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java index a0dfd19355..57c388f119 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java similarity index 100% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java similarity index 100% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java similarity index 100% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java similarity index 99% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java index fbc5ddbee4..6c9b56ad4d 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java index cc1df38c80..32c2873957 100644 --- a/wpilibj/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); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java similarity index 100% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java similarity index 100% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java similarity index 97% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java index 6212059e59..8289d4dc66 100644 --- a/wpilibj/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); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java index f74e7b4246..0b87a64467 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java index 9a57720167..67cddcfa1a 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java index 8525cf6aa1..9e28b0c1c9 100644 --- a/wpilibj/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 diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java similarity index 100% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java similarity index 100% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java index dfa2583e46..6758d3d015 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java similarity index 100% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java similarity index 98% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java index 8ca63f4d3c..693bfd5efc 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java similarity index 97% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java index b338c3fdf0..5962404e3a 100644 --- a/wpilibj/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. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Units.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/util/Units.java similarity index 97% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Units.java rename to wpimath/src/main/java/edu/wpi/first/wpilibj/util/Units.java index 1df7d33e2c..3f48306202 100644 --- a/wpilibj/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. */ diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Drake.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Drake.java similarity index 100% rename from wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Drake.java rename to wpimath/src/main/java/edu/wpi/first/wpiutil/math/Drake.java diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/DrakeJNI.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/DrakeJNI.java similarity index 95% rename from wpiutil/src/main/java/edu/wpi/first/wpiutil/math/DrakeJNI.java rename to wpimath/src/main/java/edu/wpi/first/wpiutil/math/DrakeJNI.java index d2c706ff9b..288384efa6 100644 --- a/wpiutil/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; diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java similarity index 96% rename from wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java rename to wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java index a5490a33d4..7db7454bf9 100644 --- a/wpiutil/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. */ diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java similarity index 100% rename from wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java rename to wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java similarity index 99% rename from wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java rename to wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java index 5780498b30..ffb165c22b 100644 --- a/wpiutil/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. */ diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java similarity index 97% rename from wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java rename to wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java index ac4deb9320..4ed38b073b 100644 --- a/wpiutil/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. */ diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Num.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Num.java similarity index 91% rename from wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Num.java rename to wpimath/src/main/java/edu/wpi/first/wpiutil/math/Num.java index c7385ea62c..0b8a81f329 100644 --- a/wpiutil/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. */ diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java similarity index 99% rename from wpiutil/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java rename to wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java index ea983d47ec..6ace8b2669 100644 --- a/wpiutil/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. */ diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java similarity index 92% rename from wpiutil/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java rename to wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java index deaaa41725..fb20a90107 100644 --- a/wpiutil/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. */ diff --git a/wpimath/src/main/native/cpp/MathShared.cpp b/wpimath/src/main/native/cpp/MathShared.cpp new file mode 100644 index 0000000000..3d394aeeaf --- /dev/null +++ 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 + +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; +static wpi::mutex setLock; + +MathShared& MathSharedStore::GetMathShared() { + std::scoped_lock lock(setLock); + if (!mathShared) mathShared = std::make_unique(); + return *mathShared; +} + +void MathSharedStore::SetMathShared(std::unique_ptr shared) { + std::scoped_lock lock(setLock); + mathShared = std::move(shared); +} diff --git a/wpiutil/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp b/wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp similarity index 100% rename from wpiutil/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp rename to wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp diff --git a/wpiutil/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp b/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp similarity index 100% rename from wpiutil/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp rename to wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp diff --git a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp similarity index 98% rename from wpilibc/src/main/native/cpp/geometry/Pose2d.cpp rename to wpimath/src/main/native/cpp/geometry/Pose2d.cpp index 1754830303..57dad446b5 100644 --- a/wpilibc/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. */ diff --git a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp similarity index 99% rename from wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp rename to wpimath/src/main/native/cpp/geometry/Rotation2d.cpp index 13a57d89f8..32a9b408c6 100644 --- a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp @@ -9,9 +9,10 @@ #include -#include #include +#include "units/math.h" + using namespace frc; Rotation2d::Rotation2d(units::radian_t value) diff --git a/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp similarity index 100% rename from wpilibc/src/main/native/cpp/geometry/Transform2d.cpp rename to wpimath/src/main/native/cpp/geometry/Transform2d.cpp diff --git a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp similarity index 99% rename from wpilibc/src/main/native/cpp/geometry/Translation2d.cpp rename to wpimath/src/main/native/cpp/geometry/Translation2d.cpp index cc59a3fce0..f081ac0fd4 100644 --- a/wpilibc/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 #include +#include "units/math.h" + using namespace frc; Translation2d::Translation2d(units::meter_t x, units::meter_t y) diff --git a/wpiutil/src/main/native/cpp/jni/DrakeJNI.cpp b/wpimath/src/main/native/cpp/jni/DrakeJNI.cpp similarity index 97% rename from wpiutil/src/main/native/cpp/jni/DrakeJNI.cpp rename to wpimath/src/main/native/cpp/jni/DrakeJNI.cpp index 8e5f4da4f9..2b4a8cae9d 100644 --- a/wpiutil/src/main/native/cpp/jni/DrakeJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/DrakeJNI.cpp @@ -7,14 +7,12 @@ #include -#include - #include #include +#include #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; diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp similarity index 87% rename from wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp rename to wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp index 07a8e3bba2..8df4445e4c 100644 --- a/wpilibc/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 +#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, diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp similarity index 97% rename from wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp rename to wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp index 60b8e7a22d..36a4952955 100644 --- a/wpilibc/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 +#include "units/math.h" using namespace frc; diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp similarity index 100% rename from wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp rename to wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp similarity index 87% rename from wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp rename to wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp index 38434838f1..d2adc4773c 100644 --- a/wpilibc/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 +#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( diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp similarity index 98% rename from wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp rename to wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp index 776f4efa78..b20dddf7b2 100644 --- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp @@ -11,7 +11,7 @@ #include #include -#include +#include "units/math.h" using namespace frc; diff --git a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp similarity index 97% rename from wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp rename to wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp index 3991fec4d2..c00a362979 100644 --- a/wpilibc/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. */ diff --git a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp similarity index 97% rename from wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp rename to wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp index bb8ad3c320..5b34cdb2fa 100644 --- a/wpilibc/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. */ diff --git a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp similarity index 100% rename from wpilibc/src/main/native/cpp/spline/SplineHelper.cpp rename to wpimath/src/main/native/cpp/spline/SplineHelper.cpp diff --git a/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp b/wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp similarity index 91% rename from wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp rename to wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp index f3c42c6d22..b7e7f9e487 100644 --- a/wpilibc/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. */ diff --git a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp similarity index 99% rename from wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp rename to wpimath/src/main/native/cpp/trajectory/Trajectory.cpp index 060996ece9..e52e39f51c 100644 --- a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp +++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp @@ -7,9 +7,10 @@ #include "frc/trajectory/Trajectory.h" -#include #include +#include "units/math.h" + using namespace frc; bool Trajectory::State::operator==(const Trajectory::State& other) const { diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp similarity index 100% rename from wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp rename to wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp similarity index 99% rename from wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp rename to wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp index 26d1146f6e..0e78a15d92 100644 --- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp +++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp @@ -33,7 +33,7 @@ #include -#include +#include "units/math.h" using namespace frc; diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp similarity index 96% rename from wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp rename to wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp index f3cf30d893..0ae43f5af9 100644 --- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp +++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.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. */ diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp similarity index 98% rename from wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp rename to wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp index eb42cc19e7..f04b9d6b7f 100644 --- a/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp +++ b/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp @@ -7,7 +7,7 @@ #include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h" -#include +#include "units/math.h" using namespace frc; diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp similarity index 100% rename from wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp rename to wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp similarity index 99% rename from wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp rename to wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp index 17b3b31ac3..9b0cb84d64 100644 --- a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp +++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp @@ -10,9 +10,10 @@ #include #include -#include #include +#include "units/math.h" + using namespace frc; DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint( diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp similarity index 98% rename from wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp rename to wpimath/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp index 9ddfb352cb..56fe5e9e5e 100644 --- a/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp +++ b/wpimath/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp @@ -9,7 +9,7 @@ #include -#include +#include "units/math.h" using namespace frc; diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp similarity index 98% rename from wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp rename to wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp index 07eed8cc0f..493c2696eb 100644 --- a/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp +++ b/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp @@ -7,7 +7,7 @@ #include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h" -#include +#include "units/math.h" using namespace frc; diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp similarity index 100% rename from wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp rename to wpimath/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/Cholesky b/wpimath/src/main/native/include/Eigen/Cholesky similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/Cholesky rename to wpimath/src/main/native/include/Eigen/Cholesky diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/Core b/wpimath/src/main/native/include/Eigen/Core similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/Core rename to wpimath/src/main/native/include/Eigen/Core diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/Eigen b/wpimath/src/main/native/include/Eigen/Eigen similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/Eigen rename to wpimath/src/main/native/include/Eigen/Eigen diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/Eigenvalues b/wpimath/src/main/native/include/Eigen/Eigenvalues similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/Eigenvalues rename to wpimath/src/main/native/include/Eigen/Eigenvalues diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/Householder b/wpimath/src/main/native/include/Eigen/Householder similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/Householder rename to wpimath/src/main/native/include/Eigen/Householder diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/Jacobi b/wpimath/src/main/native/include/Eigen/Jacobi similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/Jacobi rename to wpimath/src/main/native/include/Eigen/Jacobi diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/LU b/wpimath/src/main/native/include/Eigen/LU similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/LU rename to wpimath/src/main/native/include/Eigen/LU diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/QR b/wpimath/src/main/native/include/Eigen/QR similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/QR rename to wpimath/src/main/native/include/Eigen/QR diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/SVD b/wpimath/src/main/native/include/Eigen/SVD similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/SVD rename to wpimath/src/main/native/include/Eigen/SVD diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/StdDeque b/wpimath/src/main/native/include/Eigen/StdDeque similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/StdDeque rename to wpimath/src/main/native/include/Eigen/StdDeque diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/StdList b/wpimath/src/main/native/include/Eigen/StdList similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/StdList rename to wpimath/src/main/native/include/Eigen/StdList diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/StdVector b/wpimath/src/main/native/include/Eigen/StdVector similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/StdVector rename to wpimath/src/main/native/include/Eigen/StdVector diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h b/wpimath/src/main/native/include/Eigen/src/Cholesky/LDLT.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h rename to wpimath/src/main/native/include/Eigen/src/Cholesky/LDLT.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h b/wpimath/src/main/native/include/Eigen/src/Cholesky/LLT.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h rename to wpimath/src/main/native/include/Eigen/src/Cholesky/LLT.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Array.h b/wpimath/src/main/native/include/Eigen/src/Core/Array.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Array.h rename to wpimath/src/main/native/include/Eigen/src/Core/Array.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h b/wpimath/src/main/native/include/Eigen/src/Core/ArrayBase.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h rename to wpimath/src/main/native/include/Eigen/src/Core/ArrayBase.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h b/wpimath/src/main/native/include/Eigen/src/Core/ArrayWrapper.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h rename to wpimath/src/main/native/include/Eigen/src/Core/ArrayWrapper.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Assign.h b/wpimath/src/main/native/include/Eigen/src/Core/Assign.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Assign.h rename to wpimath/src/main/native/include/Eigen/src/Core/Assign.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h b/wpimath/src/main/native/include/Eigen/src/Core/AssignEvaluator.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h rename to wpimath/src/main/native/include/Eigen/src/Core/AssignEvaluator.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Assign_MKL.h b/wpimath/src/main/native/include/Eigen/src/Core/Assign_MKL.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Assign_MKL.h rename to wpimath/src/main/native/include/Eigen/src/Core/Assign_MKL.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h b/wpimath/src/main/native/include/Eigen/src/Core/BandMatrix.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h rename to wpimath/src/main/native/include/Eigen/src/Core/BandMatrix.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Block.h b/wpimath/src/main/native/include/Eigen/src/Core/Block.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Block.h rename to wpimath/src/main/native/include/Eigen/src/Core/Block.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h b/wpimath/src/main/native/include/Eigen/src/Core/BooleanRedux.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h rename to wpimath/src/main/native/include/Eigen/src/Core/BooleanRedux.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h b/wpimath/src/main/native/include/Eigen/src/Core/CommaInitializer.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h rename to wpimath/src/main/native/include/Eigen/src/Core/CommaInitializer.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ConditionEstimator.h b/wpimath/src/main/native/include/Eigen/src/Core/ConditionEstimator.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ConditionEstimator.h rename to wpimath/src/main/native/include/Eigen/src/Core/ConditionEstimator.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h b/wpimath/src/main/native/include/Eigen/src/Core/CoreEvaluators.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h rename to wpimath/src/main/native/include/Eigen/src/Core/CoreEvaluators.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h b/wpimath/src/main/native/include/Eigen/src/Core/CoreIterators.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h rename to wpimath/src/main/native/include/Eigen/src/Core/CoreIterators.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h b/wpimath/src/main/native/include/Eigen/src/Core/CwiseBinaryOp.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h rename to wpimath/src/main/native/include/Eigen/src/Core/CwiseBinaryOp.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h b/wpimath/src/main/native/include/Eigen/src/Core/CwiseNullaryOp.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h rename to wpimath/src/main/native/include/Eigen/src/Core/CwiseNullaryOp.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseTernaryOp.h b/wpimath/src/main/native/include/Eigen/src/Core/CwiseTernaryOp.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseTernaryOp.h rename to wpimath/src/main/native/include/Eigen/src/Core/CwiseTernaryOp.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h b/wpimath/src/main/native/include/Eigen/src/Core/CwiseUnaryOp.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h rename to wpimath/src/main/native/include/Eigen/src/Core/CwiseUnaryOp.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h b/wpimath/src/main/native/include/Eigen/src/Core/CwiseUnaryView.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h rename to wpimath/src/main/native/include/Eigen/src/Core/CwiseUnaryView.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h b/wpimath/src/main/native/include/Eigen/src/Core/DenseBase.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h rename to wpimath/src/main/native/include/Eigen/src/Core/DenseBase.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h b/wpimath/src/main/native/include/Eigen/src/Core/DenseCoeffsBase.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h rename to wpimath/src/main/native/include/Eigen/src/Core/DenseCoeffsBase.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h b/wpimath/src/main/native/include/Eigen/src/Core/DenseStorage.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h rename to wpimath/src/main/native/include/Eigen/src/Core/DenseStorage.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h b/wpimath/src/main/native/include/Eigen/src/Core/Diagonal.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h rename to wpimath/src/main/native/include/Eigen/src/Core/Diagonal.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h b/wpimath/src/main/native/include/Eigen/src/Core/DiagonalMatrix.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h rename to wpimath/src/main/native/include/Eigen/src/Core/DiagonalMatrix.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h b/wpimath/src/main/native/include/Eigen/src/Core/DiagonalProduct.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h rename to wpimath/src/main/native/include/Eigen/src/Core/DiagonalProduct.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Dot.h b/wpimath/src/main/native/include/Eigen/src/Core/Dot.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Dot.h rename to wpimath/src/main/native/include/Eigen/src/Core/Dot.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h b/wpimath/src/main/native/include/Eigen/src/Core/EigenBase.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h rename to wpimath/src/main/native/include/Eigen/src/Core/EigenBase.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h b/wpimath/src/main/native/include/Eigen/src/Core/ForceAlignedAccess.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h rename to wpimath/src/main/native/include/Eigen/src/Core/ForceAlignedAccess.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h b/wpimath/src/main/native/include/Eigen/src/Core/Fuzzy.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h rename to wpimath/src/main/native/include/Eigen/src/Core/Fuzzy.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h b/wpimath/src/main/native/include/Eigen/src/Core/GeneralProduct.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h rename to wpimath/src/main/native/include/Eigen/src/Core/GeneralProduct.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h b/wpimath/src/main/native/include/Eigen/src/Core/GenericPacketMath.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h rename to wpimath/src/main/native/include/Eigen/src/Core/GenericPacketMath.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h b/wpimath/src/main/native/include/Eigen/src/Core/GlobalFunctions.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h rename to wpimath/src/main/native/include/Eigen/src/Core/GlobalFunctions.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/IO.h b/wpimath/src/main/native/include/Eigen/src/Core/IO.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/IO.h rename to wpimath/src/main/native/include/Eigen/src/Core/IO.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h b/wpimath/src/main/native/include/Eigen/src/Core/Inverse.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h rename to wpimath/src/main/native/include/Eigen/src/Core/Inverse.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Map.h b/wpimath/src/main/native/include/Eigen/src/Core/Map.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Map.h rename to wpimath/src/main/native/include/Eigen/src/Core/Map.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h b/wpimath/src/main/native/include/Eigen/src/Core/MapBase.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h rename to wpimath/src/main/native/include/Eigen/src/Core/MapBase.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h b/wpimath/src/main/native/include/Eigen/src/Core/MathFunctions.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h rename to wpimath/src/main/native/include/Eigen/src/Core/MathFunctions.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h b/wpimath/src/main/native/include/Eigen/src/Core/MathFunctionsImpl.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h rename to wpimath/src/main/native/include/Eigen/src/Core/MathFunctionsImpl.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h b/wpimath/src/main/native/include/Eigen/src/Core/Matrix.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h rename to wpimath/src/main/native/include/Eigen/src/Core/Matrix.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h b/wpimath/src/main/native/include/Eigen/src/Core/MatrixBase.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h rename to wpimath/src/main/native/include/Eigen/src/Core/MatrixBase.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h b/wpimath/src/main/native/include/Eigen/src/Core/NestByValue.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h rename to wpimath/src/main/native/include/Eigen/src/Core/NestByValue.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h b/wpimath/src/main/native/include/Eigen/src/Core/NoAlias.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h rename to wpimath/src/main/native/include/Eigen/src/Core/NoAlias.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h b/wpimath/src/main/native/include/Eigen/src/Core/NumTraits.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h rename to wpimath/src/main/native/include/Eigen/src/Core/NumTraits.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h b/wpimath/src/main/native/include/Eigen/src/Core/PermutationMatrix.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h rename to wpimath/src/main/native/include/Eigen/src/Core/PermutationMatrix.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h b/wpimath/src/main/native/include/Eigen/src/Core/PlainObjectBase.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h rename to wpimath/src/main/native/include/Eigen/src/Core/PlainObjectBase.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Product.h b/wpimath/src/main/native/include/Eigen/src/Core/Product.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Product.h rename to wpimath/src/main/native/include/Eigen/src/Core/Product.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h b/wpimath/src/main/native/include/Eigen/src/Core/ProductEvaluators.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h rename to wpimath/src/main/native/include/Eigen/src/Core/ProductEvaluators.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Random.h b/wpimath/src/main/native/include/Eigen/src/Core/Random.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Random.h rename to wpimath/src/main/native/include/Eigen/src/Core/Random.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Redux.h b/wpimath/src/main/native/include/Eigen/src/Core/Redux.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Redux.h rename to wpimath/src/main/native/include/Eigen/src/Core/Redux.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Ref.h b/wpimath/src/main/native/include/Eigen/src/Core/Ref.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Ref.h rename to wpimath/src/main/native/include/Eigen/src/Core/Ref.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h b/wpimath/src/main/native/include/Eigen/src/Core/Replicate.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h rename to wpimath/src/main/native/include/Eigen/src/Core/Replicate.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h b/wpimath/src/main/native/include/Eigen/src/Core/ReturnByValue.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h rename to wpimath/src/main/native/include/Eigen/src/Core/ReturnByValue.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h b/wpimath/src/main/native/include/Eigen/src/Core/Reverse.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h rename to wpimath/src/main/native/include/Eigen/src/Core/Reverse.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Select.h b/wpimath/src/main/native/include/Eigen/src/Core/Select.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Select.h rename to wpimath/src/main/native/include/Eigen/src/Core/Select.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h b/wpimath/src/main/native/include/Eigen/src/Core/SelfAdjointView.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h rename to wpimath/src/main/native/include/Eigen/src/Core/SelfAdjointView.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SelfCwiseBinaryOp.h b/wpimath/src/main/native/include/Eigen/src/Core/SelfCwiseBinaryOp.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SelfCwiseBinaryOp.h rename to wpimath/src/main/native/include/Eigen/src/Core/SelfCwiseBinaryOp.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Solve.h b/wpimath/src/main/native/include/Eigen/src/Core/Solve.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Solve.h rename to wpimath/src/main/native/include/Eigen/src/Core/Solve.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h b/wpimath/src/main/native/include/Eigen/src/Core/SolveTriangular.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h rename to wpimath/src/main/native/include/Eigen/src/Core/SolveTriangular.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h b/wpimath/src/main/native/include/Eigen/src/Core/SolverBase.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h rename to wpimath/src/main/native/include/Eigen/src/Core/SolverBase.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h b/wpimath/src/main/native/include/Eigen/src/Core/StableNorm.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h rename to wpimath/src/main/native/include/Eigen/src/Core/StableNorm.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Stride.h b/wpimath/src/main/native/include/Eigen/src/Core/Stride.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Stride.h rename to wpimath/src/main/native/include/Eigen/src/Core/Stride.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Swap.h b/wpimath/src/main/native/include/Eigen/src/Core/Swap.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Swap.h rename to wpimath/src/main/native/include/Eigen/src/Core/Swap.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h b/wpimath/src/main/native/include/Eigen/src/Core/Transpose.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h rename to wpimath/src/main/native/include/Eigen/src/Core/Transpose.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h b/wpimath/src/main/native/include/Eigen/src/Core/Transpositions.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h rename to wpimath/src/main/native/include/Eigen/src/Core/Transpositions.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h b/wpimath/src/main/native/include/Eigen/src/Core/TriangularMatrix.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h rename to wpimath/src/main/native/include/Eigen/src/Core/TriangularMatrix.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h b/wpimath/src/main/native/include/Eigen/src/Core/VectorBlock.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h rename to wpimath/src/main/native/include/Eigen/src/Core/VectorBlock.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h b/wpimath/src/main/native/include/Eigen/src/Core/VectorwiseOp.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h rename to wpimath/src/main/native/include/Eigen/src/Core/VectorwiseOp.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h b/wpimath/src/main/native/include/Eigen/src/Core/Visitor.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h rename to wpimath/src/main/native/include/Eigen/src/Core/Visitor.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h b/wpimath/src/main/native/include/Eigen/src/Core/arch/AVX/Complex.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h rename to wpimath/src/main/native/include/Eigen/src/Core/arch/AVX/Complex.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h b/wpimath/src/main/native/include/Eigen/src/Core/arch/AVX/MathFunctions.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h rename to wpimath/src/main/native/include/Eigen/src/Core/arch/AVX/MathFunctions.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h b/wpimath/src/main/native/include/Eigen/src/Core/arch/AVX/PacketMath.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h rename to wpimath/src/main/native/include/Eigen/src/Core/arch/AVX/PacketMath.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h b/wpimath/src/main/native/include/Eigen/src/Core/arch/AVX/TypeCasting.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h rename to wpimath/src/main/native/include/Eigen/src/Core/arch/AVX/TypeCasting.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h b/wpimath/src/main/native/include/Eigen/src/Core/arch/Default/ConjHelper.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h rename to wpimath/src/main/native/include/Eigen/src/Core/arch/Default/ConjHelper.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h b/wpimath/src/main/native/include/Eigen/src/Core/arch/Default/Settings.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h rename to wpimath/src/main/native/include/Eigen/src/Core/arch/Default/Settings.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h b/wpimath/src/main/native/include/Eigen/src/Core/arch/NEON/Complex.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h rename to wpimath/src/main/native/include/Eigen/src/Core/arch/NEON/Complex.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h b/wpimath/src/main/native/include/Eigen/src/Core/arch/NEON/MathFunctions.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h rename to wpimath/src/main/native/include/Eigen/src/Core/arch/NEON/MathFunctions.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h b/wpimath/src/main/native/include/Eigen/src/Core/arch/NEON/PacketMath.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h rename to wpimath/src/main/native/include/Eigen/src/Core/arch/NEON/PacketMath.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h b/wpimath/src/main/native/include/Eigen/src/Core/arch/SSE/Complex.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h rename to wpimath/src/main/native/include/Eigen/src/Core/arch/SSE/Complex.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h b/wpimath/src/main/native/include/Eigen/src/Core/arch/SSE/MathFunctions.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h rename to wpimath/src/main/native/include/Eigen/src/Core/arch/SSE/MathFunctions.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h b/wpimath/src/main/native/include/Eigen/src/Core/arch/SSE/PacketMath.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h rename to wpimath/src/main/native/include/Eigen/src/Core/arch/SSE/PacketMath.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h b/wpimath/src/main/native/include/Eigen/src/Core/arch/SSE/TypeCasting.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h rename to wpimath/src/main/native/include/Eigen/src/Core/arch/SSE/TypeCasting.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h b/wpimath/src/main/native/include/Eigen/src/Core/functors/AssignmentFunctors.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h rename to wpimath/src/main/native/include/Eigen/src/Core/functors/AssignmentFunctors.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h b/wpimath/src/main/native/include/Eigen/src/Core/functors/BinaryFunctors.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h rename to wpimath/src/main/native/include/Eigen/src/Core/functors/BinaryFunctors.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h b/wpimath/src/main/native/include/Eigen/src/Core/functors/NullaryFunctors.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h rename to wpimath/src/main/native/include/Eigen/src/Core/functors/NullaryFunctors.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h b/wpimath/src/main/native/include/Eigen/src/Core/functors/StlFunctors.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h rename to wpimath/src/main/native/include/Eigen/src/Core/functors/StlFunctors.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/TernaryFunctors.h b/wpimath/src/main/native/include/Eigen/src/Core/functors/TernaryFunctors.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/TernaryFunctors.h rename to wpimath/src/main/native/include/Eigen/src/Core/functors/TernaryFunctors.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h b/wpimath/src/main/native/include/Eigen/src/Core/functors/UnaryFunctors.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h rename to wpimath/src/main/native/include/Eigen/src/Core/functors/UnaryFunctors.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/wpimath/src/main/native/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h rename to wpimath/src/main/native/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h b/wpimath/src/main/native/include/Eigen/src/Core/products/GeneralMatrixMatrix.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h rename to wpimath/src/main/native/include/Eigen/src/Core/products/GeneralMatrixMatrix.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h b/wpimath/src/main/native/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h rename to wpimath/src/main/native/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h b/wpimath/src/main/native/include/Eigen/src/Core/products/GeneralMatrixVector.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h rename to wpimath/src/main/native/include/Eigen/src/Core/products/GeneralMatrixVector.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h b/wpimath/src/main/native/include/Eigen/src/Core/products/Parallelizer.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h rename to wpimath/src/main/native/include/Eigen/src/Core/products/Parallelizer.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h b/wpimath/src/main/native/include/Eigen/src/Core/products/SelfadjointMatrixMatrix.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h rename to wpimath/src/main/native/include/Eigen/src/Core/products/SelfadjointMatrixMatrix.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h b/wpimath/src/main/native/include/Eigen/src/Core/products/SelfadjointMatrixVector.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h rename to wpimath/src/main/native/include/Eigen/src/Core/products/SelfadjointMatrixVector.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h b/wpimath/src/main/native/include/Eigen/src/Core/products/SelfadjointProduct.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h rename to wpimath/src/main/native/include/Eigen/src/Core/products/SelfadjointProduct.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h b/wpimath/src/main/native/include/Eigen/src/Core/products/SelfadjointRank2Update.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h rename to wpimath/src/main/native/include/Eigen/src/Core/products/SelfadjointRank2Update.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h b/wpimath/src/main/native/include/Eigen/src/Core/products/TriangularMatrixMatrix.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h rename to wpimath/src/main/native/include/Eigen/src/Core/products/TriangularMatrixMatrix.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixVector.h b/wpimath/src/main/native/include/Eigen/src/Core/products/TriangularMatrixVector.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixVector.h rename to wpimath/src/main/native/include/Eigen/src/Core/products/TriangularMatrixVector.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h b/wpimath/src/main/native/include/Eigen/src/Core/products/TriangularSolverMatrix.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h rename to wpimath/src/main/native/include/Eigen/src/Core/products/TriangularSolverMatrix.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h b/wpimath/src/main/native/include/Eigen/src/Core/products/TriangularSolverVector.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h rename to wpimath/src/main/native/include/Eigen/src/Core/products/TriangularSolverVector.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h b/wpimath/src/main/native/include/Eigen/src/Core/util/BlasUtil.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h rename to wpimath/src/main/native/include/Eigen/src/Core/util/BlasUtil.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h b/wpimath/src/main/native/include/Eigen/src/Core/util/Constants.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h rename to wpimath/src/main/native/include/Eigen/src/Core/util/Constants.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h b/wpimath/src/main/native/include/Eigen/src/Core/util/DisableStupidWarnings.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h rename to wpimath/src/main/native/include/Eigen/src/Core/util/DisableStupidWarnings.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h b/wpimath/src/main/native/include/Eigen/src/Core/util/ForwardDeclarations.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h rename to wpimath/src/main/native/include/Eigen/src/Core/util/ForwardDeclarations.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h b/wpimath/src/main/native/include/Eigen/src/Core/util/Macros.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h rename to wpimath/src/main/native/include/Eigen/src/Core/util/Macros.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h b/wpimath/src/main/native/include/Eigen/src/Core/util/Memory.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h rename to wpimath/src/main/native/include/Eigen/src/Core/util/Memory.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h b/wpimath/src/main/native/include/Eigen/src/Core/util/Meta.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h rename to wpimath/src/main/native/include/Eigen/src/Core/util/Meta.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h b/wpimath/src/main/native/include/Eigen/src/Core/util/ReenableStupidWarnings.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h rename to wpimath/src/main/native/include/Eigen/src/Core/util/ReenableStupidWarnings.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h b/wpimath/src/main/native/include/Eigen/src/Core/util/StaticAssert.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h rename to wpimath/src/main/native/include/Eigen/src/Core/util/StaticAssert.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h b/wpimath/src/main/native/include/Eigen/src/Core/util/XprHelper.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h rename to wpimath/src/main/native/include/Eigen/src/Core/util/XprHelper.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h b/wpimath/src/main/native/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h rename to wpimath/src/main/native/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h b/wpimath/src/main/native/include/Eigen/src/Eigenvalues/ComplexSchur.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h rename to wpimath/src/main/native/include/Eigen/src/Eigenvalues/ComplexSchur.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h b/wpimath/src/main/native/include/Eigen/src/Eigenvalues/EigenSolver.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h rename to wpimath/src/main/native/include/Eigen/src/Eigenvalues/EigenSolver.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h b/wpimath/src/main/native/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h rename to wpimath/src/main/native/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h b/wpimath/src/main/native/include/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h rename to wpimath/src/main/native/include/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h b/wpimath/src/main/native/include/Eigen/src/Eigenvalues/HessenbergDecomposition.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h rename to wpimath/src/main/native/include/Eigen/src/Eigenvalues/HessenbergDecomposition.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h b/wpimath/src/main/native/include/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h rename to wpimath/src/main/native/include/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h b/wpimath/src/main/native/include/Eigen/src/Eigenvalues/RealQZ.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h rename to wpimath/src/main/native/include/Eigen/src/Eigenvalues/RealQZ.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h b/wpimath/src/main/native/include/Eigen/src/Eigenvalues/RealSchur.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h rename to wpimath/src/main/native/include/Eigen/src/Eigenvalues/RealSchur.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/wpimath/src/main/native/include/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h rename to wpimath/src/main/native/include/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h b/wpimath/src/main/native/include/Eigen/src/Eigenvalues/Tridiagonalization.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h rename to wpimath/src/main/native/include/Eigen/src/Eigenvalues/Tridiagonalization.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h b/wpimath/src/main/native/include/Eigen/src/Householder/BlockHouseholder.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h rename to wpimath/src/main/native/include/Eigen/src/Householder/BlockHouseholder.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h b/wpimath/src/main/native/include/Eigen/src/Householder/Householder.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h rename to wpimath/src/main/native/include/Eigen/src/Householder/Householder.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h b/wpimath/src/main/native/include/Eigen/src/Householder/HouseholderSequence.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h rename to wpimath/src/main/native/include/Eigen/src/Householder/HouseholderSequence.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h b/wpimath/src/main/native/include/Eigen/src/Jacobi/Jacobi.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h rename to wpimath/src/main/native/include/Eigen/src/Jacobi/Jacobi.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h b/wpimath/src/main/native/include/Eigen/src/LU/Determinant.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h rename to wpimath/src/main/native/include/Eigen/src/LU/Determinant.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h b/wpimath/src/main/native/include/Eigen/src/LU/FullPivLU.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h rename to wpimath/src/main/native/include/Eigen/src/LU/FullPivLU.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h b/wpimath/src/main/native/include/Eigen/src/LU/InverseImpl.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h rename to wpimath/src/main/native/include/Eigen/src/LU/InverseImpl.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h b/wpimath/src/main/native/include/Eigen/src/LU/PartialPivLU.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h rename to wpimath/src/main/native/include/Eigen/src/LU/PartialPivLU.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/LU/arch/Inverse_SSE.h b/wpimath/src/main/native/include/Eigen/src/LU/arch/Inverse_SSE.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/LU/arch/Inverse_SSE.h rename to wpimath/src/main/native/include/Eigen/src/LU/arch/Inverse_SSE.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h b/wpimath/src/main/native/include/Eigen/src/QR/ColPivHouseholderQR.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h rename to wpimath/src/main/native/include/Eigen/src/QR/ColPivHouseholderQR.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h b/wpimath/src/main/native/include/Eigen/src/QR/CompleteOrthogonalDecomposition.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h rename to wpimath/src/main/native/include/Eigen/src/QR/CompleteOrthogonalDecomposition.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h b/wpimath/src/main/native/include/Eigen/src/QR/FullPivHouseholderQR.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h rename to wpimath/src/main/native/include/Eigen/src/QR/FullPivHouseholderQR.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h b/wpimath/src/main/native/include/Eigen/src/QR/HouseholderQR.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h rename to wpimath/src/main/native/include/Eigen/src/QR/HouseholderQR.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h b/wpimath/src/main/native/include/Eigen/src/SVD/BDCSVD.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h rename to wpimath/src/main/native/include/Eigen/src/SVD/BDCSVD.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h b/wpimath/src/main/native/include/Eigen/src/SVD/JacobiSVD.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h rename to wpimath/src/main/native/include/Eigen/src/SVD/JacobiSVD.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h b/wpimath/src/main/native/include/Eigen/src/SVD/SVDBase.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h rename to wpimath/src/main/native/include/Eigen/src/SVD/SVDBase.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h b/wpimath/src/main/native/include/Eigen/src/SVD/UpperBidiagonalization.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h rename to wpimath/src/main/native/include/Eigen/src/SVD/UpperBidiagonalization.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h b/wpimath/src/main/native/include/Eigen/src/StlSupport/StdDeque.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h rename to wpimath/src/main/native/include/Eigen/src/StlSupport/StdDeque.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h b/wpimath/src/main/native/include/Eigen/src/StlSupport/StdList.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h rename to wpimath/src/main/native/include/Eigen/src/StlSupport/StdList.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h b/wpimath/src/main/native/include/Eigen/src/StlSupport/StdVector.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h rename to wpimath/src/main/native/include/Eigen/src/StlSupport/StdVector.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/details.h b/wpimath/src/main/native/include/Eigen/src/StlSupport/details.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/details.h rename to wpimath/src/main/native/include/Eigen/src/StlSupport/details.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/misc/Image.h b/wpimath/src/main/native/include/Eigen/src/misc/Image.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/misc/Image.h rename to wpimath/src/main/native/include/Eigen/src/misc/Image.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/misc/Kernel.h b/wpimath/src/main/native/include/Eigen/src/misc/Kernel.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/misc/Kernel.h rename to wpimath/src/main/native/include/Eigen/src/misc/Kernel.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/misc/RealSvd2x2.h b/wpimath/src/main/native/include/Eigen/src/misc/RealSvd2x2.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/misc/RealSvd2x2.h rename to wpimath/src/main/native/include/Eigen/src/misc/RealSvd2x2.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h b/wpimath/src/main/native/include/Eigen/src/plugins/ArrayCwiseBinaryOps.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h rename to wpimath/src/main/native/include/Eigen/src/plugins/ArrayCwiseBinaryOps.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h b/wpimath/src/main/native/include/Eigen/src/plugins/ArrayCwiseUnaryOps.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h rename to wpimath/src/main/native/include/Eigen/src/plugins/ArrayCwiseUnaryOps.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h b/wpimath/src/main/native/include/Eigen/src/plugins/BlockMethods.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h rename to wpimath/src/main/native/include/Eigen/src/plugins/BlockMethods.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseBinaryOps.h b/wpimath/src/main/native/include/Eigen/src/plugins/CommonCwiseBinaryOps.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseBinaryOps.h rename to wpimath/src/main/native/include/Eigen/src/plugins/CommonCwiseBinaryOps.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h b/wpimath/src/main/native/include/Eigen/src/plugins/CommonCwiseUnaryOps.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h rename to wpimath/src/main/native/include/Eigen/src/plugins/CommonCwiseUnaryOps.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h b/wpimath/src/main/native/include/Eigen/src/plugins/MatrixCwiseBinaryOps.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h rename to wpimath/src/main/native/include/Eigen/src/plugins/MatrixCwiseBinaryOps.h diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h b/wpimath/src/main/native/include/Eigen/src/plugins/MatrixCwiseUnaryOps.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h rename to wpimath/src/main/native/include/Eigen/src/plugins/MatrixCwiseUnaryOps.h diff --git a/wpiutil/src/main/native/include/drake/common/drake_assert.h b/wpimath/src/main/native/include/drake/common/drake_assert.h similarity index 100% rename from wpiutil/src/main/native/include/drake/common/drake_assert.h rename to wpimath/src/main/native/include/drake/common/drake_assert.h diff --git a/wpiutil/src/main/native/include/drake/common/drake_assertion_error.h b/wpimath/src/main/native/include/drake/common/drake_assertion_error.h similarity index 100% rename from wpiutil/src/main/native/include/drake/common/drake_assertion_error.h rename to wpimath/src/main/native/include/drake/common/drake_assertion_error.h diff --git a/wpiutil/src/main/native/include/drake/common/drake_copyable.h b/wpimath/src/main/native/include/drake/common/drake_copyable.h similarity index 100% rename from wpiutil/src/main/native/include/drake/common/drake_copyable.h rename to wpimath/src/main/native/include/drake/common/drake_copyable.h diff --git a/wpiutil/src/main/native/include/drake/common/drake_throw.h b/wpimath/src/main/native/include/drake/common/drake_throw.h similarity index 100% rename from wpiutil/src/main/native/include/drake/common/drake_throw.h rename to wpimath/src/main/native/include/drake/common/drake_throw.h diff --git a/wpiutil/src/main/native/include/drake/common/is_approx_equal_abstol.h b/wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h similarity index 100% rename from wpiutil/src/main/native/include/drake/common/is_approx_equal_abstol.h rename to wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h diff --git a/wpiutil/src/main/native/include/drake/common/never_destroyed.h b/wpimath/src/main/native/include/drake/common/never_destroyed.h similarity index 100% rename from wpiutil/src/main/native/include/drake/common/never_destroyed.h rename to wpimath/src/main/native/include/drake/common/never_destroyed.h diff --git a/wpiutil/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h similarity index 100% rename from wpiutil/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h rename to wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h diff --git a/wpilibc/src/main/native/include/frc/LinearFilter.h b/wpimath/src/main/native/include/frc/LinearFilter.h similarity index 97% rename from wpilibc/src/main/native/include/frc/LinearFilter.h rename to wpimath/src/main/native/include/frc/LinearFilter.h index 4c9c859d53..a8ce8b5ca6 100644 --- a/wpilibc/src/main/native/include/frc/LinearFilter.h +++ b/wpimath/src/main/native/include/frc/LinearFilter.h @@ -12,11 +12,12 @@ #include #include -#include -#include #include #include +#include "math/MathShared.h" +#include "units/time.h" + namespace frc { /** @@ -85,7 +86,8 @@ class LinearFilter { m_outputGains(fbGains) { static int instances = 0; instances++; - HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kFilter_Linear, 1); } /** diff --git a/wpilibc/src/main/native/include/frc/MedianFilter.h b/wpimath/src/main/native/include/frc/MedianFilter.h similarity index 97% rename from wpilibc/src/main/native/include/frc/MedianFilter.h rename to wpimath/src/main/native/include/frc/MedianFilter.h index b5d499b238..3ccccbf65a 100644 --- a/wpilibc/src/main/native/include/frc/MedianFilter.h +++ b/wpimath/src/main/native/include/frc/MedianFilter.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. */ diff --git a/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h similarity index 98% rename from wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h rename to wpimath/src/main/native/include/frc/controller/ArmFeedforward.h index 6629de1253..14df18786f 100644 --- a/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h @@ -7,12 +7,13 @@ #pragma once -#include -#include -#include -#include #include +#include "units/angle.h" +#include "units/angular_velocity.h" +#include "units/math.h" +#include "units/voltage.h" + namespace frc { /** * A helper class that computes feedforward outputs for a simple arm (modeled as diff --git a/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h similarity index 99% rename from wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h rename to wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h index faf8ad786f..b82d9602e3 100644 --- a/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h @@ -7,9 +7,10 @@ #pragma once -#include #include +#include "units/voltage.h" + namespace frc { /** * A helper class that computes feedforward outputs for a simple elevator diff --git a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h similarity index 99% rename from wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h rename to wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index 1d31a25c15..1afab40da5 100644 --- a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -7,10 +7,11 @@ #pragma once -#include -#include #include +#include "units/time.h" +#include "units/voltage.h" + namespace frc { /** * A helper class that computes feedforward voltages for a simple diff --git a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h similarity index 100% rename from wpilibc/src/main/native/include/frc/geometry/Pose2d.h rename to wpimath/src/main/native/include/frc/geometry/Pose2d.h diff --git a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h similarity index 100% rename from wpilibc/src/main/native/include/frc/geometry/Rotation2d.h rename to wpimath/src/main/native/include/frc/geometry/Rotation2d.h diff --git a/wpilibc/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h similarity index 100% rename from wpilibc/src/main/native/include/frc/geometry/Transform2d.h rename to wpimath/src/main/native/include/frc/geometry/Transform2d.h diff --git a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h similarity index 100% rename from wpilibc/src/main/native/include/frc/geometry/Translation2d.h rename to wpimath/src/main/native/include/frc/geometry/Translation2d.h diff --git a/wpilibc/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h similarity index 100% rename from wpilibc/src/main/native/include/frc/geometry/Twist2d.h rename to wpimath/src/main/native/include/frc/geometry/Twist2d.h diff --git a/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h similarity index 97% rename from wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h rename to wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h index a87a11909b..1716ca7b18 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h @@ -7,10 +7,9 @@ #pragma once -#include -#include - #include "frc/geometry/Rotation2d.h" +#include "units/angular_velocity.h" +#include "units/velocity.h" namespace frc { /** diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h similarity index 92% rename from wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h rename to wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h index 5de3ec6a55..790fdf9267 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h @@ -7,12 +7,11 @@ #pragma once -#include -#include -#include - #include "frc/kinematics/ChassisSpeeds.h" #include "frc/kinematics/DifferentialDriveWheelSpeeds.h" +#include "math/MathShared.h" +#include "units/angle.h" +#include "units/length.h" namespace frc { /** @@ -35,8 +34,8 @@ class DifferentialDriveKinematics { */ explicit DifferentialDriveKinematics(units::meter_t trackWidth) : trackWidth(trackWidth) { - HAL_Report(HALUsageReporting::kResourceType_Kinematics, - HALUsageReporting::kKinematics_DifferentialDrive); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kKinematics_DifferentialDrive, 1); } /** diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h similarity index 98% rename from wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h rename to wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h index b1023be2dd..a65b52a559 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h @@ -7,11 +7,9 @@ #pragma once -#include - #include "DifferentialDriveKinematics.h" #include "frc/geometry/Pose2d.h" -#include "frc2/Timer.h" +#include "units/length.h" namespace frc { /** diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h similarity index 98% rename from wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h rename to wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h index 8eec16b9ef..20085bbbf3 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h @@ -7,7 +7,7 @@ #pragma once -#include +#include "units/velocity.h" namespace frc { /** diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h similarity index 97% rename from wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h rename to wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h index ccddc871ed..26df9ca00d 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h @@ -9,11 +9,11 @@ #include #include -#include #include "frc/geometry/Translation2d.h" #include "frc/kinematics/ChassisSpeeds.h" #include "frc/kinematics/MecanumDriveWheelSpeeds.h" +#include "math/MathShared.h" namespace frc { @@ -64,8 +64,8 @@ class MecanumDriveKinematics { SetInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel, rearRightWheel); m_forwardKinematics = m_inverseKinematics.householderQr(); - HAL_Report(HALUsageReporting::kResourceType_Kinematics, - HALUsageReporting::kKinematics_MecanumDrive); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kKinematics_MecanumDrive, 1); } MecanumDriveKinematics(const MecanumDriveKinematics&) = default; diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h similarity index 96% rename from wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h rename to wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h index 3008d99053..546a4989ad 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h @@ -7,12 +7,12 @@ #pragma once -#include +#include #include "frc/geometry/Pose2d.h" #include "frc/kinematics/MecanumDriveKinematics.h" #include "frc/kinematics/MecanumDriveWheelSpeeds.h" -#include "frc2/Timer.h" +#include "units/time.h" namespace frc { @@ -92,8 +92,7 @@ class MecanumDriveOdometry { */ const Pose2d& Update(const Rotation2d& gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) { - return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle, - wheelSpeeds); + return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, wheelSpeeds); } private: diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h similarity index 98% rename from wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h rename to wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h index 7e45e1554d..aa82b99a71 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h @@ -7,7 +7,7 @@ #pragma once -#include +#include "units/velocity.h" namespace frc { /** diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h similarity index 97% rename from wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h rename to wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 53b1330d65..83168d465c 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -12,13 +12,13 @@ #include #include -#include -#include #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Translation2d.h" #include "frc/kinematics/ChassisSpeeds.h" #include "frc/kinematics/SwerveModuleState.h" +#include "math/MathShared.h" +#include "units/velocity.h" namespace frc { /** @@ -73,8 +73,8 @@ class SwerveDriveKinematics { m_forwardKinematics = m_inverseKinematics.householderQr(); - HAL_Report(HALUsageReporting::kResourceType_Kinematics, - HALUsageReporting::kKinematics_SwerveDrive); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kKinematics_SwerveDrive, 1); } SwerveDriveKinematics(const SwerveDriveKinematics&) = default; diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc similarity index 99% rename from wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc rename to wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc index d40b668fc1..08eba50112 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc @@ -9,7 +9,7 @@ #include -#include +#include "units/math.h" namespace frc { diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h similarity index 96% rename from wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h rename to wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h index c2f6460a35..03591da4cd 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h @@ -11,12 +11,12 @@ #include #include -#include +#include #include "SwerveDriveKinematics.h" #include "SwerveModuleState.h" #include "frc/geometry/Pose2d.h" -#include "frc2/Timer.h" +#include "units/time.h" namespace frc { @@ -103,8 +103,7 @@ class SwerveDriveOdometry { template const Pose2d& Update(const Rotation2d& gyroAngle, ModuleStates&&... moduleStates) { - return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle, - moduleStates...); + return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, moduleStates...); } private: diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc similarity index 87% rename from wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc rename to wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc index e79438866e..dd08de5f9d 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc @@ -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 @@ #pragma once -#include +#include "math/MathShared.h" namespace frc { template @@ -17,8 +17,8 @@ SwerveDriveOdometry::SwerveDriveOdometry( : m_kinematics(kinematics), m_pose(initialPose) { m_previousAngle = m_pose.Rotation(); m_gyroOffset = m_pose.Rotation() - gyroAngle; - HAL_Report(HALUsageReporting::kResourceType_Odometry, - HALUsageReporting::kOdometry_SwerveDrive); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kOdometry_SwerveDrive, 1); } template diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h similarity index 96% rename from wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h rename to wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h index f7aa4d9ac7..b5ae7f1cb0 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h @@ -7,9 +7,8 @@ #pragma once -#include - #include "frc/geometry/Rotation2d.h" +#include "units/velocity.h" namespace frc { /** diff --git a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h similarity index 97% rename from wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h rename to wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h index 1e08a942e8..5a153e2bab 100644 --- a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.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. */ diff --git a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h similarity index 98% rename from wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h rename to wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h index 632d3adcf7..7fb34dc2b9 100644 --- a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.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. */ diff --git a/wpilibc/src/main/native/include/frc/spline/Spline.h b/wpimath/src/main/native/include/frc/spline/Spline.h similarity index 98% rename from wpilibc/src/main/native/include/frc/spline/Spline.h rename to wpimath/src/main/native/include/frc/spline/Spline.h index cf16bf5195..b2b8829d34 100644 --- a/wpilibc/src/main/native/include/frc/spline/Spline.h +++ b/wpimath/src/main/native/include/frc/spline/Spline.h @@ -12,10 +12,10 @@ #include #include -#include -#include #include "frc/geometry/Pose2d.h" +#include "units/curvature.h" +#include "units/length.h" namespace frc { /** diff --git a/wpilibc/src/main/native/include/frc/spline/SplineHelper.h b/wpimath/src/main/native/include/frc/spline/SplineHelper.h similarity index 100% rename from wpilibc/src/main/native/include/frc/spline/SplineHelper.h rename to wpimath/src/main/native/include/frc/spline/SplineHelper.h diff --git a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h b/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h similarity index 97% rename from wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h rename to wpimath/src/main/native/include/frc/spline/SplineParameterizer.h index 97334ab17c..8e7079cb12 100644 --- a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h +++ b/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h @@ -31,19 +31,19 @@ #pragma once -#include - #include #include #include #include -#include -#include -#include -#include #include +#include "frc/spline/Spline.h" +#include "units/angle.h" +#include "units/curvature.h" +#include "units/length.h" +#include "units/math.h" + namespace frc { /** diff --git a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h similarity index 97% rename from wpilibc/src/main/native/include/frc/trajectory/Trajectory.h rename to wpimath/src/main/native/include/frc/trajectory/Trajectory.h index 2ca386272f..023b2c2da8 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h +++ b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h @@ -9,13 +9,12 @@ #include -#include -#include -#include -#include - #include "frc/geometry/Pose2d.h" #include "frc/geometry/Transform2d.h" +#include "units/acceleration.h" +#include "units/curvature.h" +#include "units/time.h" +#include "units/velocity.h" namespace wpi { class json; diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h similarity index 98% rename from wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h rename to wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h index 291bd98177..5bd7977051 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h +++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h @@ -11,9 +11,6 @@ #include #include -#include -#include - #include "frc/kinematics/DifferentialDriveKinematics.h" #include "frc/kinematics/MecanumDriveKinematics.h" #include "frc/kinematics/SwerveDriveKinematics.h" @@ -21,6 +18,8 @@ #include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h" #include "frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" +#include "units/acceleration.h" +#include "units/velocity.h" namespace frc { /** diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h similarity index 100% rename from wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h rename to wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h similarity index 100% rename from wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h rename to wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h similarity index 96% rename from wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h rename to wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h index 700ed9c214..f05cd14694 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h +++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.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. */ diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h similarity index 94% rename from wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h rename to wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h index 00be35c7db..3246ea269d 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h +++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h @@ -7,8 +7,8 @@ #pragma once -#include -#include +#include "math/MathShared.h" +#include "units/time.h" namespace frc { @@ -55,11 +55,13 @@ class TrapezoidProfile { class Constraints { public: Constraints() { - HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kTrajectory_TrapezoidProfile, 1); } Constraints(Velocity_t maxVelocity_, Acceleration_t maxAcceleration_) : maxVelocity{maxVelocity_}, maxAcceleration{maxAcceleration_} { - HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1); + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kTrajectory_TrapezoidProfile, 1); } Velocity_t maxVelocity{0}; Acceleration_t maxAcceleration{0}; diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc similarity index 99% rename from wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc rename to wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc index 51d1c17548..8718cc0c3f 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc +++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc @@ -9,7 +9,7 @@ #include -#include +#include "units/math.h" namespace frc { template diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h similarity index 94% rename from wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h rename to wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h index 8dc865dc33..4f897ba7cc 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h @@ -7,11 +7,10 @@ #pragma once -#include -#include -#include - #include "frc/trajectory/constraint/TrajectoryConstraint.h" +#include "units/acceleration.h" +#include "units/curvature.h" +#include "units/velocity.h" namespace frc { diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h similarity index 98% rename from wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h rename to wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h index 268805c866..27a35bbff4 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h @@ -7,10 +7,9 @@ #pragma once -#include - #include "frc/kinematics/DifferentialDriveKinematics.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" +#include "units/velocity.h" namespace frc { /** diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h similarity index 97% rename from wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h rename to wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h index bb2336731f..2776bf5747 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h @@ -7,12 +7,11 @@ #pragma once -#include -#include - #include "frc/controller/SimpleMotorFeedforward.h" #include "frc/kinematics/DifferentialDriveKinematics.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" +#include "units/length.h" +#include "units/voltage.h" namespace frc { /** diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h similarity index 98% rename from wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h rename to wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h index 9441144f99..e28d2aa1e3 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h @@ -7,11 +7,10 @@ #pragma once -#include - #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Translation2d.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" +#include "units/length.h" namespace frc { /** diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h similarity index 96% rename from wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h rename to wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h index 8a4ed557cf..7a30881f62 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h @@ -7,10 +7,9 @@ #pragma once -#include -#include - #include "frc/trajectory/constraint/TrajectoryConstraint.h" +#include "units/math.h" +#include "units/velocity.h" namespace frc { /** diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h similarity index 98% rename from wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h rename to wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h index 8f235d5ca5..741d915c38 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h @@ -9,10 +9,9 @@ #include -#include - #include "frc/kinematics/MecanumDriveKinematics.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" +#include "units/velocity.h" namespace frc { /** diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h similarity index 100% rename from wpilibc/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h rename to wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h similarity index 98% rename from wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h rename to wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h index d808fc34e2..0e93f0f3af 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h @@ -9,10 +9,9 @@ #include -#include - #include "frc/kinematics/SwerveDriveKinematics.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" +#include "units/velocity.h" namespace frc { diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc similarity index 100% rename from wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc rename to wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h similarity index 96% rename from wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h rename to wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h index ae9390560f..b5548c5008 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h @@ -9,12 +9,11 @@ #include -#include -#include -#include - #include "frc/geometry/Pose2d.h" #include "frc/spline/Spline.h" +#include "units/acceleration.h" +#include "units/curvature.h" +#include "units/velocity.h" namespace frc { /** diff --git a/wpimath/src/main/native/include/math/MathShared.h b/wpimath/src/main/native/include/math/MathShared.h new file mode 100644 index 0000000000..ea23a8830b --- /dev/null +++ b/wpimath/src/main/native/include/math/MathShared.h @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +namespace wpi::math { + +enum class MathUsageId { + kKinematics_DifferentialDrive, + kKinematics_MecanumDrive, + kKinematics_SwerveDrive, + kTrajectory_TrapezoidProfile, + kFilter_Linear, + kOdometry_DifferentialDrive, + kOdometry_SwerveDrive, + kOdometry_MecanumDrive +}; + +class MathShared { + public: + virtual ~MathShared() = default; + virtual void ReportError(const wpi::Twine& error) = 0; + virtual void ReportUsage(MathUsageId id, int count) = 0; +}; + +class MathSharedStore { + public: + static MathShared& GetMathShared(); + + static void SetMathShared(std::unique_ptr shared); + + static void ReportError(const wpi::Twine& error) { + GetMathShared().ReportError(error); + } + + static void ReportUsage(MathUsageId id, int count) { + GetMathShared().ReportUsage(id, count); + } +}; + +} // namespace wpi::math diff --git a/wpiutil/src/main/native/include/units/acceleration.h b/wpimath/src/main/native/include/units/acceleration.h similarity index 100% rename from wpiutil/src/main/native/include/units/acceleration.h rename to wpimath/src/main/native/include/units/acceleration.h diff --git a/wpiutil/src/main/native/include/units/angle.h b/wpimath/src/main/native/include/units/angle.h similarity index 100% rename from wpiutil/src/main/native/include/units/angle.h rename to wpimath/src/main/native/include/units/angle.h diff --git a/wpiutil/src/main/native/include/units/angular_acceleration.h b/wpimath/src/main/native/include/units/angular_acceleration.h similarity index 100% rename from wpiutil/src/main/native/include/units/angular_acceleration.h rename to wpimath/src/main/native/include/units/angular_acceleration.h diff --git a/wpiutil/src/main/native/include/units/angular_velocity.h b/wpimath/src/main/native/include/units/angular_velocity.h similarity index 100% rename from wpiutil/src/main/native/include/units/angular_velocity.h rename to wpimath/src/main/native/include/units/angular_velocity.h diff --git a/wpiutil/src/main/native/include/units/area.h b/wpimath/src/main/native/include/units/area.h similarity index 100% rename from wpiutil/src/main/native/include/units/area.h rename to wpimath/src/main/native/include/units/area.h diff --git a/wpiutil/src/main/native/include/units/base.h b/wpimath/src/main/native/include/units/base.h similarity index 100% rename from wpiutil/src/main/native/include/units/base.h rename to wpimath/src/main/native/include/units/base.h diff --git a/wpiutil/src/main/native/include/units/capacitance.h b/wpimath/src/main/native/include/units/capacitance.h similarity index 100% rename from wpiutil/src/main/native/include/units/capacitance.h rename to wpimath/src/main/native/include/units/capacitance.h diff --git a/wpiutil/src/main/native/include/units/charge.h b/wpimath/src/main/native/include/units/charge.h similarity index 100% rename from wpiutil/src/main/native/include/units/charge.h rename to wpimath/src/main/native/include/units/charge.h diff --git a/wpiutil/src/main/native/include/units/concentration.h b/wpimath/src/main/native/include/units/concentration.h similarity index 100% rename from wpiutil/src/main/native/include/units/concentration.h rename to wpimath/src/main/native/include/units/concentration.h diff --git a/wpiutil/src/main/native/include/units/conductance.h b/wpimath/src/main/native/include/units/conductance.h similarity index 100% rename from wpiutil/src/main/native/include/units/conductance.h rename to wpimath/src/main/native/include/units/conductance.h diff --git a/wpiutil/src/main/native/include/units/constants.h b/wpimath/src/main/native/include/units/constants.h similarity index 100% rename from wpiutil/src/main/native/include/units/constants.h rename to wpimath/src/main/native/include/units/constants.h diff --git a/wpiutil/src/main/native/include/units/current.h b/wpimath/src/main/native/include/units/current.h similarity index 100% rename from wpiutil/src/main/native/include/units/current.h rename to wpimath/src/main/native/include/units/current.h diff --git a/wpiutil/src/main/native/include/units/curvature.h b/wpimath/src/main/native/include/units/curvature.h similarity index 100% rename from wpiutil/src/main/native/include/units/curvature.h rename to wpimath/src/main/native/include/units/curvature.h diff --git a/wpiutil/src/main/native/include/units/data.h b/wpimath/src/main/native/include/units/data.h similarity index 100% rename from wpiutil/src/main/native/include/units/data.h rename to wpimath/src/main/native/include/units/data.h diff --git a/wpiutil/src/main/native/include/units/data_transfer_rate.h b/wpimath/src/main/native/include/units/data_transfer_rate.h similarity index 100% rename from wpiutil/src/main/native/include/units/data_transfer_rate.h rename to wpimath/src/main/native/include/units/data_transfer_rate.h diff --git a/wpiutil/src/main/native/include/units/density.h b/wpimath/src/main/native/include/units/density.h similarity index 100% rename from wpiutil/src/main/native/include/units/density.h rename to wpimath/src/main/native/include/units/density.h diff --git a/wpiutil/src/main/native/include/units/dimensionless.h b/wpimath/src/main/native/include/units/dimensionless.h similarity index 100% rename from wpiutil/src/main/native/include/units/dimensionless.h rename to wpimath/src/main/native/include/units/dimensionless.h diff --git a/wpiutil/src/main/native/include/units/energy.h b/wpimath/src/main/native/include/units/energy.h similarity index 100% rename from wpiutil/src/main/native/include/units/energy.h rename to wpimath/src/main/native/include/units/energy.h diff --git a/wpiutil/src/main/native/include/units/force.h b/wpimath/src/main/native/include/units/force.h similarity index 100% rename from wpiutil/src/main/native/include/units/force.h rename to wpimath/src/main/native/include/units/force.h diff --git a/wpiutil/src/main/native/include/units/frequency.h b/wpimath/src/main/native/include/units/frequency.h similarity index 100% rename from wpiutil/src/main/native/include/units/frequency.h rename to wpimath/src/main/native/include/units/frequency.h diff --git a/wpiutil/src/main/native/include/units/illuminance.h b/wpimath/src/main/native/include/units/illuminance.h similarity index 100% rename from wpiutil/src/main/native/include/units/illuminance.h rename to wpimath/src/main/native/include/units/illuminance.h diff --git a/wpiutil/src/main/native/include/units/impedance.h b/wpimath/src/main/native/include/units/impedance.h similarity index 100% rename from wpiutil/src/main/native/include/units/impedance.h rename to wpimath/src/main/native/include/units/impedance.h diff --git a/wpiutil/src/main/native/include/units/inductance.h b/wpimath/src/main/native/include/units/inductance.h similarity index 100% rename from wpiutil/src/main/native/include/units/inductance.h rename to wpimath/src/main/native/include/units/inductance.h diff --git a/wpiutil/src/main/native/include/units/length.h b/wpimath/src/main/native/include/units/length.h similarity index 100% rename from wpiutil/src/main/native/include/units/length.h rename to wpimath/src/main/native/include/units/length.h diff --git a/wpiutil/src/main/native/include/units/luminous_flux.h b/wpimath/src/main/native/include/units/luminous_flux.h similarity index 100% rename from wpiutil/src/main/native/include/units/luminous_flux.h rename to wpimath/src/main/native/include/units/luminous_flux.h diff --git a/wpiutil/src/main/native/include/units/luminous_intensity.h b/wpimath/src/main/native/include/units/luminous_intensity.h similarity index 100% rename from wpiutil/src/main/native/include/units/luminous_intensity.h rename to wpimath/src/main/native/include/units/luminous_intensity.h diff --git a/wpiutil/src/main/native/include/units/magnetic_field_strength.h b/wpimath/src/main/native/include/units/magnetic_field_strength.h similarity index 100% rename from wpiutil/src/main/native/include/units/magnetic_field_strength.h rename to wpimath/src/main/native/include/units/magnetic_field_strength.h diff --git a/wpiutil/src/main/native/include/units/magnetic_flux.h b/wpimath/src/main/native/include/units/magnetic_flux.h similarity index 100% rename from wpiutil/src/main/native/include/units/magnetic_flux.h rename to wpimath/src/main/native/include/units/magnetic_flux.h diff --git a/wpiutil/src/main/native/include/units/mass.h b/wpimath/src/main/native/include/units/mass.h similarity index 100% rename from wpiutil/src/main/native/include/units/mass.h rename to wpimath/src/main/native/include/units/mass.h diff --git a/wpiutil/src/main/native/include/units/math.h b/wpimath/src/main/native/include/units/math.h similarity index 100% rename from wpiutil/src/main/native/include/units/math.h rename to wpimath/src/main/native/include/units/math.h diff --git a/wpiutil/src/main/native/include/units/moment_of_inertia.h b/wpimath/src/main/native/include/units/moment_of_inertia.h similarity index 100% rename from wpiutil/src/main/native/include/units/moment_of_inertia.h rename to wpimath/src/main/native/include/units/moment_of_inertia.h diff --git a/wpiutil/src/main/native/include/units/power.h b/wpimath/src/main/native/include/units/power.h similarity index 100% rename from wpiutil/src/main/native/include/units/power.h rename to wpimath/src/main/native/include/units/power.h diff --git a/wpiutil/src/main/native/include/units/pressure.h b/wpimath/src/main/native/include/units/pressure.h similarity index 100% rename from wpiutil/src/main/native/include/units/pressure.h rename to wpimath/src/main/native/include/units/pressure.h diff --git a/wpiutil/src/main/native/include/units/radiation.h b/wpimath/src/main/native/include/units/radiation.h similarity index 100% rename from wpiutil/src/main/native/include/units/radiation.h rename to wpimath/src/main/native/include/units/radiation.h diff --git a/wpiutil/src/main/native/include/units/solid_angle.h b/wpimath/src/main/native/include/units/solid_angle.h similarity index 100% rename from wpiutil/src/main/native/include/units/solid_angle.h rename to wpimath/src/main/native/include/units/solid_angle.h diff --git a/wpiutil/src/main/native/include/units/substance.h b/wpimath/src/main/native/include/units/substance.h similarity index 100% rename from wpiutil/src/main/native/include/units/substance.h rename to wpimath/src/main/native/include/units/substance.h diff --git a/wpiutil/src/main/native/include/units/temperature.h b/wpimath/src/main/native/include/units/temperature.h similarity index 100% rename from wpiutil/src/main/native/include/units/temperature.h rename to wpimath/src/main/native/include/units/temperature.h diff --git a/wpiutil/src/main/native/include/units/time.h b/wpimath/src/main/native/include/units/time.h similarity index 100% rename from wpiutil/src/main/native/include/units/time.h rename to wpimath/src/main/native/include/units/time.h diff --git a/wpiutil/src/main/native/include/units/torque.h b/wpimath/src/main/native/include/units/torque.h similarity index 100% rename from wpiutil/src/main/native/include/units/torque.h rename to wpimath/src/main/native/include/units/torque.h diff --git a/wpiutil/src/main/native/include/units/units.h b/wpimath/src/main/native/include/units/units.h similarity index 100% rename from wpiutil/src/main/native/include/units/units.h rename to wpimath/src/main/native/include/units/units.h diff --git a/wpiutil/src/main/native/include/units/velocity.h b/wpimath/src/main/native/include/units/velocity.h similarity index 100% rename from wpiutil/src/main/native/include/units/velocity.h rename to wpimath/src/main/native/include/units/velocity.h diff --git a/wpiutil/src/main/native/include/units/voltage.h b/wpimath/src/main/native/include/units/voltage.h similarity index 100% rename from wpiutil/src/main/native/include/units/voltage.h rename to wpimath/src/main/native/include/units/voltage.h diff --git a/wpiutil/src/main/native/include/units/volume.h b/wpimath/src/main/native/include/units/volume.h similarity index 100% rename from wpiutil/src/main/native/include/units/volume.h rename to wpimath/src/main/native/include/units/volume.h diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff b/wpimath/src/main/native/include/unsupported/Eigen/AutoDiff similarity index 100% rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff rename to wpimath/src/main/native/include/unsupported/Eigen/AutoDiff diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/MatrixFunctions b/wpimath/src/main/native/include/unsupported/Eigen/MatrixFunctions similarity index 100% rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/MatrixFunctions rename to wpimath/src/main/native/include/unsupported/Eigen/MatrixFunctions diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h b/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h rename to wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h b/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h rename to wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h b/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h rename to wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h rename to wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h b/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h rename to wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h b/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h rename to wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h b/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h rename to wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h b/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h rename to wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/StemFunction.h b/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h similarity index 100% rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/StemFunction.h rename to wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java similarity index 98% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java index 3953c4cce7..da58b29f46 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.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. */ diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java similarity index 96% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java index 00b100d3c7..b2b596cc48 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.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. */ diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java new file mode 100644 index 0000000000..05c5786bad --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java @@ -0,0 +1,61 @@ +/*----------------------------------------------------------------------------*/ +/* 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.wpilibj; + +import java.lang.reflect.Constructor; +import java.lang.reflect.InvocationTargetException; +import java.lang.reflect.Modifier; +import java.util.Arrays; +import java.util.stream.Stream; + +import org.junit.jupiter.api.DynamicTest; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.TestFactory; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.DynamicTest.dynamicTest; + +@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod") +public abstract class UtilityClassTest { + private final Class m_clazz; + + protected UtilityClassTest(Class clazz) { + m_clazz = clazz; + } + + @Test + public void singleConstructorTest() { + assertEquals(1, m_clazz.getDeclaredConstructors().length, + "More than one constructor defined"); + } + + @Test + public void constructorPrivateTest() { + Constructor constructor = m_clazz.getDeclaredConstructors()[0]; + + assertFalse(constructor.canAccess(null), "Constructor is not private"); + } + + @Test + public void constructorReflectionTest() { + Constructor constructor = m_clazz.getDeclaredConstructors()[0]; + constructor.setAccessible(true); + assertThrows(InvocationTargetException.class, constructor::newInstance); + } + + @TestFactory + Stream publicMethodsStaticTestFactory() { + return Arrays.stream(m_clazz.getDeclaredMethods()) + .filter(method -> Modifier.isPublic(method.getModifiers())) + .map(method -> dynamicTest(method.getName(), + () -> assertTrue(Modifier.isStatic(method.getModifiers())))); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java similarity index 100% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java similarity index 97% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java index 6c4b1b3a4d..8a08944fa7 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.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. */ diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java similarity index 100% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java similarity index 97% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java index b4e29475f2..dc109692bc 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.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. */ diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java similarity index 100% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java similarity index 94% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java index 729d7b84c4..b06abe6fc9 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.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. */ diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java similarity index 98% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java index e484eabc25..9d2ad4e7dc 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.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. */ diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java similarity index 100% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java similarity index 99% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java index 75b6f448b7..86a3687bd9 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.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. */ diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java similarity index 100% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java similarity index 99% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java index f506436a40..5cbec50b3b 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.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. */ diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java similarity index 100% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java similarity index 100% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java similarity index 100% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java similarity index 96% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java index 977c5abdd0..0c2f4f10ee 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.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. */ diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java similarity index 97% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java index d025860a2d..6ed99668b5 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.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. */ diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java similarity index 100% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java similarity index 100% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java similarity index 100% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java similarity index 95% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java index 19e3fa09a6..2bfa972f3e 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java @@ -17,7 +17,6 @@ import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Transform2d; import edu.wpi.first.wpilibj.geometry.Translation2d; -import edu.wpi.first.wpilibj.simulation.DriverStationSim; import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint; import static edu.wpi.first.wpilibj.util.Units.feetToMeters; @@ -75,8 +74,6 @@ class TrajectoryGeneratorTest { @Test void testMalformedTrajectory() { - DriverStationSim.setSendError(false); - var traj = TrajectoryGenerator.generateTrajectory( Arrays.asList( @@ -88,7 +85,5 @@ class TrajectoryGeneratorTest { assertEquals(traj.getStates().size(), 1); assertEquals(traj.getTotalTimeSeconds(), 0); - - DriverStationSim.setSendError(true); } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java similarity index 95% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java index e389a4fb79..d8d59b16ee 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.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. */ diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java similarity index 97% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java index 19f31dc9f6..b17046b339 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.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. */ diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java similarity index 99% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java index 090eaccfcd..3552e41248 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.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. */ diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java similarity index 96% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java rename to wpimath/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java index 1d000461ba..993866051c 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.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. */ diff --git a/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeJNITest.java b/wpimath/src/test/java/edu/wpi/first/wpiutil/math/DrakeJNITest.java similarity index 100% rename from wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeJNITest.java rename to wpimath/src/test/java/edu/wpi/first/wpiutil/math/DrakeJNITest.java diff --git a/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeTest.java b/wpimath/src/test/java/edu/wpi/first/wpiutil/math/DrakeTest.java similarity index 100% rename from wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeTest.java rename to wpimath/src/test/java/edu/wpi/first/wpiutil/math/DrakeTest.java diff --git a/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java similarity index 100% rename from wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java rename to wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java diff --git a/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java b/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java similarity index 98% rename from wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java rename to wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java index 4d6697d3b1..8ec462f2df 100644 --- a/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.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. */ diff --git a/wpiutil/src/test/native/cpp/EigenTest.cpp b/wpimath/src/test/native/cpp/EigenTest.cpp similarity index 96% rename from wpiutil/src/test/native/cpp/EigenTest.cpp rename to wpimath/src/test/native/cpp/EigenTest.cpp index 04a0bc2543..26b5da1199 100644 --- a/wpiutil/src/test/native/cpp/EigenTest.cpp +++ b/wpimath/src/test/native/cpp/EigenTest.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. */ diff --git a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp similarity index 99% rename from wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp rename to wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp index 637decb9fc..934e14b8ec 100644 --- a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp +++ b/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp @@ -11,10 +11,10 @@ #include #include -#include #include #include "gtest/gtest.h" +#include "units/time.h" // Filter constants static constexpr units::second_t kFilterStep = 0.005_s; diff --git a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp similarity index 99% rename from wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp rename to wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp index a20437ed24..d321518376 100644 --- a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp +++ b/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp @@ -12,10 +12,10 @@ #include #include -#include #include #include "gtest/gtest.h" +#include "units/time.h" // Filter constants static constexpr units::second_t kFilterStep = 0.005_s; diff --git a/wpilibc/src/test/native/cpp/MedianFilterTest.cpp b/wpimath/src/test/native/cpp/MedianFilterTest.cpp similarity index 95% rename from wpilibc/src/test/native/cpp/MedianFilterTest.cpp rename to wpimath/src/test/native/cpp/MedianFilterTest.cpp index 7fe7d2e2a4..2a02e1c69d 100644 --- a/wpilibc/src/test/native/cpp/MedianFilterTest.cpp +++ b/wpimath/src/test/native/cpp/MedianFilterTest.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. */ diff --git a/wpiutil/src/test/native/cpp/UnitsTest.cpp b/wpimath/src/test/native/cpp/UnitsTest.cpp similarity index 99% rename from wpiutil/src/test/native/cpp/UnitsTest.cpp rename to wpimath/src/test/native/cpp/UnitsTest.cpp index 1bfd22029d..54e8195f99 100644 --- a/wpiutil/src/test/native/cpp/UnitsTest.cpp +++ b/wpimath/src/test/native/cpp/UnitsTest.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. */ diff --git a/wpiutil/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp b/wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp similarity index 100% rename from wpiutil/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp rename to wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp diff --git a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp similarity index 100% rename from wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp rename to wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp diff --git a/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp similarity index 96% rename from wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp rename to wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index ba807870b0..a29371f1dc 100644 --- a/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.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. */ diff --git a/wpilibc/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp similarity index 100% rename from wpilibc/src/test/native/cpp/geometry/Transform2dTest.cpp rename to wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp diff --git a/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp similarity index 97% rename from wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp rename to wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp index 99fced719e..3eb5c78345 100644 --- a/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.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. */ diff --git a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp similarity index 100% rename from wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp rename to wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp diff --git a/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp similarity index 93% rename from wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp rename to wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp index 3fb63fbd87..864860a11e 100644 --- a/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.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. */ diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp similarity index 97% rename from wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp rename to wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp index c77b634401..7c1a28d511 100644 --- a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp @@ -5,14 +5,14 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include -#include -#include #include #include "frc/kinematics/ChassisSpeeds.h" #include "frc/kinematics/DifferentialDriveKinematics.h" #include "gtest/gtest.h" +#include "units/angular_velocity.h" +#include "units/length.h" +#include "units/velocity.h" using namespace frc; diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp similarity index 100% rename from wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp rename to wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp similarity index 99% rename from wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp rename to wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp index af5ba1597d..cb03d9706f 100644 --- a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp @@ -5,12 +5,12 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include #include "frc/geometry/Translation2d.h" #include "frc/kinematics/MecanumDriveKinematics.h" #include "gtest/gtest.h" +#include "units/angular_velocity.h" using namespace frc; diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp similarity index 100% rename from wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp rename to wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp similarity index 99% rename from wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp rename to wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index d52eed70de..368dbaf102 100644 --- a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -5,12 +5,12 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include #include "frc/geometry/Translation2d.h" #include "frc/kinematics/SwerveDriveKinematics.h" #include "gtest/gtest.h" +#include "units/angular_velocity.h" using namespace frc; diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp similarity index 100% rename from wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp rename to wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp diff --git a/wpimath/src/test/native/cpp/main.cpp b/wpimath/src/test/native/cpp/main.cpp new file mode 100644 index 0000000000..e2126f2ef6 --- /dev/null +++ b/wpimath/src/test/native/cpp/main.cpp @@ -0,0 +1,14 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp similarity index 99% rename from wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp rename to wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp index d59ed014d8..fe084e0917 100644 --- a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp +++ b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp @@ -9,14 +9,13 @@ #include #include -#include - #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" #include "frc/spline/QuinticHermiteSpline.h" #include "frc/spline/SplineHelper.h" #include "frc/spline/SplineParameterizer.h" #include "gtest/gtest.h" +#include "units/length.h" using namespace frc; diff --git a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp similarity index 98% rename from wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp rename to wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp index 56e3b2214b..3dd70a579d 100644 --- a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp +++ b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp @@ -8,15 +8,14 @@ #include #include -#include -#include - #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" #include "frc/spline/QuinticHermiteSpline.h" #include "frc/spline/SplineHelper.h" #include "frc/spline/SplineParameterizer.h" #include "gtest/gtest.h" +#include "units/angle.h" +#include "units/length.h" using namespace frc; diff --git a/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp similarity index 93% rename from wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp rename to wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp index 3cc565baf5..42e9fe2ffc 100644 --- a/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp @@ -8,15 +8,14 @@ #include #include -#include -#include -#include -#include - #include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" #include "gtest/gtest.h" #include "trajectory/TestTrajectory.h" +#include "units/acceleration.h" +#include "units/angle.h" +#include "units/math.h" +#include "units/velocity.h" using namespace frc; diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp similarity index 98% rename from wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp rename to wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp index 304de0e8c7..636b002788 100644 --- a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp @@ -8,12 +8,11 @@ #include #include -#include - #include "frc/kinematics/DifferentialDriveKinematics.h" #include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h" #include "gtest/gtest.h" #include "trajectory/TestTrajectory.h" +#include "units/time.h" using namespace frc; diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp similarity index 96% rename from wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp rename to wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp index 9314e61ac4..6cd807581f 100644 --- a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp @@ -9,18 +9,17 @@ #include #include -#include -#include -#include -#include -#include - #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" +#include "units/acceleration.h" +#include "units/length.h" +#include "units/time.h" +#include "units/velocity.h" +#include "units/voltage.h" using namespace frc; diff --git a/wpilibc/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp similarity index 95% rename from wpilibc/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp rename to wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp index a2dc37c10f..44c4222490 100644 --- a/wpilibc/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp @@ -7,16 +7,15 @@ #include -#include -#include -#include -#include - #include "frc/kinematics/DifferentialDriveKinematics.h" #include "frc/trajectory/constraint/EllipticalRegionConstraint.h" #include "frc/trajectory/constraint/MaxVelocityConstraint.h" #include "gtest/gtest.h" #include "trajectory/TestTrajectory.h" +#include "units/acceleration.h" +#include "units/angle.h" +#include "units/length.h" +#include "units/velocity.h" using namespace frc; diff --git a/wpilibc/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp similarity index 95% rename from wpilibc/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp rename to wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp index 7d1144f4a2..33f753f0e0 100644 --- a/wpilibc/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp @@ -7,16 +7,15 @@ #include -#include -#include -#include -#include - #include "frc/kinematics/DifferentialDriveKinematics.h" #include "frc/trajectory/constraint/MaxVelocityConstraint.h" #include "frc/trajectory/constraint/RectangularRegionConstraint.h" #include "gtest/gtest.h" #include "trajectory/TestTrajectory.h" +#include "units/acceleration.h" +#include "units/length.h" +#include "units/math.h" +#include "units/velocity.h" using namespace frc; diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp similarity index 98% rename from wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp rename to wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp index 8101eef130..378aff7376 100644 --- a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp @@ -7,14 +7,13 @@ #include -#include - #include "frc/trajectory/Trajectory.h" #include "frc/trajectory/TrajectoryGenerator.h" #include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" #include "gtest/gtest.h" #include "trajectory/TestTrajectory.h" +#include "units/math.h" using namespace frc; diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp similarity index 93% rename from wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp rename to wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp index 999d2bbd85..c18a3e99d5 100644 --- a/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.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. */ diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp similarity index 100% rename from wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp rename to wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp diff --git a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp similarity index 98% rename from wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp rename to wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp index 53cae83dfc..63ea916b94 100644 --- a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp @@ -10,12 +10,11 @@ #include #include -#include -#include -#include -#include - #include "gtest/gtest.h" +#include "units/acceleration.h" +#include "units/length.h" +#include "units/math.h" +#include "units/velocity.h" static constexpr auto kDt = 10_ms; diff --git a/wpiutil/src/test/native/include/drake/common/autodiff.h b/wpimath/src/test/native/include/drake/common/autodiff.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/autodiff.h rename to wpimath/src/test/native/include/drake/common/autodiff.h diff --git a/wpiutil/src/test/native/include/drake/common/autodiff_overloads.h b/wpimath/src/test/native/include/drake/common/autodiff_overloads.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/autodiff_overloads.h rename to wpimath/src/test/native/include/drake/common/autodiff_overloads.h diff --git a/wpiutil/src/test/native/include/drake/common/autodiffxd.h b/wpimath/src/test/native/include/drake/common/autodiffxd.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/autodiffxd.h rename to wpimath/src/test/native/include/drake/common/autodiffxd.h diff --git a/wpiutil/src/test/native/include/drake/common/cond.h b/wpimath/src/test/native/include/drake/common/cond.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/cond.h rename to wpimath/src/test/native/include/drake/common/cond.h diff --git a/wpiutil/src/test/native/include/drake/common/constants.h b/wpimath/src/test/native/include/drake/common/constants.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/constants.h rename to wpimath/src/test/native/include/drake/common/constants.h diff --git a/wpiutil/src/test/native/include/drake/common/double_overloads.h b/wpimath/src/test/native/include/drake/common/double_overloads.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/double_overloads.h rename to wpimath/src/test/native/include/drake/common/double_overloads.h diff --git a/wpiutil/src/test/native/include/drake/common/drake_deprecated.h b/wpimath/src/test/native/include/drake/common/drake_deprecated.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/drake_deprecated.h rename to wpimath/src/test/native/include/drake/common/drake_deprecated.h diff --git a/wpiutil/src/test/native/include/drake/common/drake_nodiscard.h b/wpimath/src/test/native/include/drake/common/drake_nodiscard.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/drake_nodiscard.h rename to wpimath/src/test/native/include/drake/common/drake_nodiscard.h diff --git a/wpiutil/src/test/native/include/drake/common/dummy_value.h b/wpimath/src/test/native/include/drake/common/dummy_value.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/dummy_value.h rename to wpimath/src/test/native/include/drake/common/dummy_value.h diff --git a/wpiutil/src/test/native/include/drake/common/eigen_autodiff_limits.h b/wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/eigen_autodiff_limits.h rename to wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h diff --git a/wpiutil/src/test/native/include/drake/common/eigen_autodiff_types.h b/wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/eigen_autodiff_types.h rename to wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h diff --git a/wpiutil/src/test/native/include/drake/common/eigen_types.h b/wpimath/src/test/native/include/drake/common/eigen_types.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/eigen_types.h rename to wpimath/src/test/native/include/drake/common/eigen_types.h diff --git a/wpiutil/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h b/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h rename to wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h diff --git a/wpiutil/src/test/native/include/drake/common/unused.h b/wpimath/src/test/native/include/drake/common/unused.h similarity index 100% rename from wpiutil/src/test/native/include/drake/common/unused.h rename to wpimath/src/test/native/include/drake/common/unused.h diff --git a/wpiutil/src/test/native/include/drake/math/autodiff.h b/wpimath/src/test/native/include/drake/math/autodiff.h similarity index 100% rename from wpiutil/src/test/native/include/drake/math/autodiff.h rename to wpimath/src/test/native/include/drake/math/autodiff.h diff --git a/wpilibc/src/test/native/include/trajectory/TestTrajectory.h b/wpimath/src/test/native/include/trajectory/TestTrajectory.h similarity index 95% rename from wpilibc/src/test/native/include/trajectory/TestTrajectory.h rename to wpimath/src/test/native/include/trajectory/TestTrajectory.h index 4a496d76bc..1cac87f1cd 100644 --- a/wpilibc/src/test/native/include/trajectory/TestTrajectory.h +++ b/wpimath/src/test/native/include/trajectory/TestTrajectory.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. */ diff --git a/wpimath/wpimath-config.cmake.in b/wpimath/wpimath-config.cmake.in new file mode 100644 index 0000000000..2f661d99de --- /dev/null +++ b/wpimath/wpimath-config.cmake.in @@ -0,0 +1,5 @@ +include(CMakeFindDependencyMacro) +@FILENAME_DEP_REPLACE@ +@WPIUTIL_DEP_REPLACE@ + +include(${SELF_DIR}/wpimath.cmake) diff --git a/wpiutil/.styleguide b/wpiutil/.styleguide index 0b96dc5236..2edcfaad1a 100644 --- a/wpiutil/.styleguide +++ b/wpiutil/.styleguide @@ -9,15 +9,9 @@ cppSrcFileInclude { } generatedFileExclude { - src/main/native/cpp/drake/ src/main/native/cpp/http_parser\.cpp$ src/main/native/cpp/llvm/ - src/main/native/eigeninclude/ - src/main/native/include/drake/ src/main/native/include/llvm/ - src/main/native/include/units/base\.h$ - src/main/native/include/units/units\.h$ - src/main/native/include/unsupported/ src/main/native/include/wpi/AlignOf\.h$ src/main/native/include/wpi/ArrayRef\.h$ src/main/native/include/wpi/Chrono\.h$ @@ -72,10 +66,7 @@ generatedFileExclude { src/main/native/include/uv/ src/main/native/libuv/ src/main/native/resources/ - src/test/native/cpp/UnitsTest\.cpp$ src/test/native/cpp/llvm/ - src/test/native/cpp/drake/ - src/test/native/include/drake/ src/main/native/windows/StackWalker } diff --git a/wpiutil/CMakeLists.txt b/wpiutil/CMakeLists.txt index 91bbbbaf5a..0eb72b2db9 100644 --- a/wpiutil/CMakeLists.txt +++ b/wpiutil/CMakeLists.txt @@ -5,7 +5,7 @@ include(GenResources) include(CompileWarnings) include(AddTest) -file(GLOB wpiutil_jni_src src/main/native/cpp/jni/DrakeJNI.cpp src/main/native/cpp/jni/WPIUtilJNI.cpp) +file(GLOB wpiutil_jni_src src/main/native/cpp/jni/WPIUtilJNI.cpp) # Java bindings if (NOT WITHOUT_JAVA) @@ -14,30 +14,6 @@ if (NOT WITHOUT_JAVA) include(UseJava) set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked") - if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml/ejml-simple-0.38.jar") - set(BASE_URL "https://search.maven.org/remotecontent?filepath=") - set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpiutil/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() - if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/jackson-core-2.10.0.jar") set(BASE_URL "https://search.maven.org/remotecontent?filepath=") set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson") @@ -54,32 +30,20 @@ if (NOT WITHOUT_JAVA) message(STATUS "All files downloaded.") endif() - file(GLOB EJML_JARS - ${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml/*.jar) - file(GLOB JACKSON_JARS ${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar) - set(CMAKE_JAVA_INCLUDE_PATH wpiutil.jar ${EJML_JARS}) - - execute_process(COMMAND python3 ${CMAKE_SOURCE_DIR}/wpiutil/generate_numbers.py ${CMAKE_BINARY_DIR}/wpiutil RESULT_VARIABLE generateResult) - if(NOT (generateResult EQUAL "0")) - # Try python - execute_process(COMMAND python ${CMAKE_SOURCE_DIR}/wpiutil/generate_numbers.py ${CMAKE_BINARY_DIR}/wpiutil RESULT_VARIABLE generateResult) - if(NOT (generateResult EQUAL "0")) - message(FATAL_ERROR "python and python3 generate_numbers.py failed") - endif() - endif() + set(CMAKE_JAVA_INCLUDE_PATH wpiutil.jar ${JACKSON_JARS}) set(CMAKE_JNI_TARGET true) - file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java ${CMAKE_BINARY_DIR}/wpiutil/generated/*.java) + file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) if(${CMAKE_VERSION} VERSION_LESS "3.11.0") set(CMAKE_JAVA_COMPILE_FLAGS "-h" "${CMAKE_CURRENT_BINARY_DIR}/jniheaders") - add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} ${JACKSON_JARS} OUTPUT_NAME wpiutil) + add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${JACKSON_JARS} OUTPUT_NAME wpiutil) else() - add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} ${JACKSON_JARS} OUTPUT_NAME wpiutil GENERATE_NATIVE_HEADERS wpiutil_jni_headers) + add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${JACKSON_JARS} OUTPUT_NAME wpiutil GENERATE_NATIVE_HEADERS wpiutil_jni_headers) endif() get_property(WPIUTIL_JAR_FILE TARGET wpiutil_jar PROPERTY JAR_FILE) @@ -181,16 +145,6 @@ endif() wpilib_target_warnings(wpiutil) target_link_libraries(wpiutil Threads::Threads ${CMAKE_DL_LIBS} ${ATOMIC}) -if (NOT USE_VCPKG_EIGEN) - install(DIRECTORY src/main/native/eigeninclude/ DESTINATION "${include_dest}/wpiutil") - target_include_directories(wpiutil PUBLIC - $ - $) -else() - find_package(Eigen3 CONFIG REQUIRED) - target_link_libraries (wpiutil Eigen3::Eigen) -endif() - if (NOT USE_VCPKG_LIBUV) target_sources(wpiutil PRIVATE ${uv_native_src}) install(DIRECTORY src/main/native/libuv/include/ DESTINATION "${include_dest}/wpiutil") diff --git a/wpiutil/build.gradle b/wpiutil/build.gradle index fbd1d732ce..f2efe2826d 100644 --- a/wpiutil/build.gradle +++ b/wpiutil/build.gradle @@ -173,9 +173,6 @@ cppHeadersZip { from('src/main/native/libuv/include') { into '/' } - from('src/main/native/eigeninclude') { - into '/' - } } model { @@ -183,7 +180,7 @@ model { all { it.sources.each { it.exportedHeaders { - srcDirs 'src/main/native/include', 'src/main/native/libuv/include', 'src/main/native/libuv/src', 'src/main/native/eigeninclude' + srcDirs 'src/main/native/include', 'src/main/native/libuv/include', 'src/main/native/libuv/src' } } } @@ -243,68 +240,7 @@ model { } 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 diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/ErrorMessages.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/ErrorMessages.java new file mode 100644 index 0000000000..70387eb450 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/ErrorMessages.java @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* 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.wpiutil; + +import static java.util.Objects.requireNonNull; + +/** + * Utility class for common WPILib error messages. + */ +public final class ErrorMessages { + /** + * Utility class, so constructor is private. + */ + private ErrorMessages() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Requires that a parameter of a method not be null; prints an error message with + * helpful debugging instructions if the parameter is null. + * + * @param obj The parameter that must not be null. + * @param paramName The name of the parameter. + * @param methodName The name of the method. + */ + public static T requireNonNullParam(T obj, String paramName, String methodName) { + return requireNonNull(obj, + "Parameter " + paramName + " in method " + methodName + " was null when it" + + " should not have been! Check the stacktrace to find the responsible line of code - " + + "usually, it is the first line of user-written code indicated in the stacktrace. " + + "Make sure all objects passed to the method in question were properly initialized -" + + " note that this may not be obvious if it is being called under " + + "dynamically-changing conditions! Please do not seek additional technical assistance" + + " without doing this first!"); + } +}