Squashed 'third_party/allwpilib/' changes from e473a00f97..e4b91005cf
e4b91005cf [examples] Update SwerveModule constructor doc (NFC) (#4042)
a260bfd83b [examples] Remove "this" keyword from SwerveModule (#4043)
18e262a100 [examples] Fix multiple doc typos in SwerveControllerCommand example (NFC) (#4044)
4bd1f526ab [wpilibc] Prevent StopMotor from terminating robot during MotorSafety check (#4038)
27847d7eb2 [sim] Expose GUI control functions via HAL_RegisterExtension (#4034)
b2a8d3f0f3 [wpilibc] Add mechanism to reset MotorSafety list (#4037)
49adac9564 [wpilib] Check for signedness in ArcadeDriveIK() (#4028)
a19d1133b1 [wpiutil] libuv: Fix sign compare warnings in gcc 11.2 (#4031)
dde91717e4 [build] cmake: Add ability to customize target warnings (#4032)
e9050afd67 [sim] Update sim match time to match real robot (#4024)
165d2837cf [wpilib] Preferences: Set Persistent in Init methods (#4025)
ac7549edca [glass] Fix snprintf truncation warning (#4029)
4d96bc72e0 [wpilibj] Fix typos in error messages for non-null assertions (#4014)
3411eee20f [hal] Replace hardcoded sim array sizes with constants (#4015)
74de97eeca [wpilibc] Add mechanism to reset various global structures (#4007)
4e3cc25012 [examples] Fix periodic function rate comment (NFC) (#4013)
90c1db393e [sim] Add exported functions to control the sim GUI (#3995)
2f43274aa4 [wpilibj] MechanismRoot2d: Add flush to setPosition (#4011)
aeca09db09 [glass] Support remapping of Enter key (#3994)
c107f22c67 [sim] Sim GUI: don't force-show Timing and Other Devices (#4001)
68fe51e8da [wpigui] Update PFD to latest, fix kdialog multiselect (#4005)
8d08d67cf1 [wpigui] PFD: Add console warning if file chooser unavailable (#4003)
4f1782f66e [wpilibc] Only call HAL_Report when initializing SmartDashboard (#4006)
3f77725cd3 Remove uses of iostream (#4004)
5635f33a32 [glass] Increase plot depth to 20K points (#3993)
bca4b7111b [glass] Fix PlotSeries::SetSource() (#3991)
6a6366b0d6 [commands] Add until() as alias for withInterrupt() (#3981)
16bf2c70c5 [wpilib] Fix joystick out of range error messages (#3988)
4b3edb742c [wpilib] Fix ADIS16448 IMU default constructor not working in Java (#3989)
fcf23fc9e9 [hal] Fix potential gamedata out of bounds read (#3983)
af5ef510c5 [wpilibc] Fix REV PH pulse duration units (#3982)
05401e2b81 [wpilib] Write REV PH firmware version to roboRIO to display on driver station (#3977)
9fde0110b6 Update to 2022 v4.0 image (#3944)
b03f8ddb2e [examples] fix incorrect variable in Arm Simulation Pref (#3980)
a26df2a022 [examples] Update ArmSimulation example to use Preferences (#3976)
d68d6674e8 [examples] Armbot: rename kCos to kG (#3975)
a8f0f6bb90 [wpilibj] Fix ADIS16448 getRate to return rate instead of angle (#3974)
dd9c92d5bf [build] Remove debug info from examples (#3971)
84df14dd70 [rtns] Fix icons (#3972)
560094ad92 [examples] Correct Mecanum example axes (#3955)
7ea1be9c01 [wpilibc] Fix typo in hardware version for REV PDH (#3969)
700f13bffd [wpilibj] Make methods public for Java REV PDH (#3970)
b6aa7c1aa9 [wpilibj] Make methods public for Java REVPH (#3968)
eb4d183e48 [wpimath] Fix clang-tidy bugprone-integer-division warning (#3966)
77e4e81e1e [wpilib] Add Field widget to BuiltInWidgets in shuffleboard (#3961)
88f5cb6eb0 [build] Publish PDBs with C++ tools (#3960)
efae552f3e [wpimath] Remove DifferentialDriveKinematics include from odometry (#3958)
46b277421a [glass] Update Speed Controller Type name for 2022 WPILib (#3952)
42908126b9 [wpilib] Add DCMotorSim (#3910)
a467392cbd [wpiutil] StackTrace: Add ability to override default implementation (#3951)
78d0bcf49d [templates] Add SimulationInit()/SimulationPeriodic() to robot templates (#3943)
02a0ced9b0 [wpilib] MecanumDrive: update docs for axis to match implementation (NFC) (#3942)
4ccfe1c9f2 [wpilib] Added docs clarification on units for drive class WheelSpeeds (NFC) (#3939)
830c0c5c2f [wpilib] MechanismLigament2d: Add getters for color and line weight (#3947)
5548a37465 [wpilib] PowerDistribution: Add module type getter (#3948)
2f9a600de2 [hal] Fix PCM one shot (#3949)
559db11a20 [myRobot] Skip deploying debug libraries for myRobot deploys (#3950)
76c78e295b [examples] Reorder SwerveModules in SwerveControllerCommand example odometry update (#3934)
debbd5ff4b [wpilib] Improve PowerDistribution docs (NFC) (#3925)
841174f302 [commands] Change command vendordep JSON version number to 1.0.0 (#3938)
8c55844f91 [wpilib] Remove comment about Mecanum right side inverted (NFC) (#3929)
0b990bf0f5 [hal] Fix PCM sticky faults clear function crashing (#3932)
104d7e2abc [hal] Don't throw exceptions in PCM JNI (#3933)
5ba69e1af1 [examples] Updated type in Java SwerveModule (#3928)
f3a0b5c7d7 [wpimath] Fix Java SimpleMotorFeedforward Docs (NFC) (#3926)
7f4265facc [wpimath] Add LinearFilter::FiniteDifference() (#3900)
63d1fb3bed [wpiutil] Modify fmt to not throw on write failure (#3919)
36af6d25a5 [wpimath] Fix input vector in pose estimator docs (NFC) (#3923)
8f387f7255 [wpilibj] Switch ControlWord mutex to actual reentrant mutex (#3922)
792e735e08 [wpimath] Move TrajectoryGenerator::SetErrorHandler definition to .cpp (#3920)
3b76de83eb [commands] Fix ProfiledPIDCommand use-after-free (#3904)
ad9f738cfa [fieldimages] Fix maven publishing (#3897)
49455199e5 [examples] Use left/rightGroup.Get() for simulator inputs to fix inversions (#3908)
64426502ea [wpimath] Fix arm -> flywheel typo (NFC) (#3911)
8cc112d196 [wpiutil] Fix wpi::array for move-only types (#3917)
e78cd49861 [build] Upgrade Java formatter plugins (#3894)
cfb4f756d6 [build] Upgrade to shadow 7.1.2 (#3893)
ba0908216c [wpimath] Fix crash in KF latency compensator (#3888)
a3a0334fad [build] cmake: Move fieldImages to WITH_GUI (#3885)
cf7460c3a8 [fieldImages] Add 2022 field (#3883)
db0fbb6448 [wpimath] Fix LQR matrix constructor overload for Q, R, and N (#3884)
8ac45f20bb [commands] Update Command documentation (NFC) (#3881)
b3707cca0b [wpiutil] Upgrade to fmt 8.1.1 (#3879)
a69ee3ece9 [wpimath] Const-qualify Twist2d scalar multiply (#3882)
750d9a30c9 [examples] Fix Eigen out of range error when running example (#3877)
41c5b2b5ac [rtns] Add cmake build (#3866)
6cf3f9b28e [build] Upgrade to Gradle 7.3.3 (#3878)
269cf03472 [examples] Add communication examples (e.g. arduino) (#2500)
5ccfc4adbd [oldcommands] Deprecate PIDWrappers, since they use deprecated interfaces (#3868)
b6f44f98be [hal] Add warning about onboard I2C (#3871)
0dca57e9ec [templates] romieducational: Invert drivetrain and disable motor safety (#3869)
22c4da152e [wpilib] Add GetRate() to ADIS classes (#3864)
05d66f862d [templates] Change the template ordering to put command based first (#3863)
b09f5b2cf2 [wpilibc] Add virtual dtor for LinearSystemSim (#3861)
a2510aaa0e [wpilib] Make ADIS IMU classes unit-safe (#3860)
947f589916 [wpilibc] Rename ADIS_16470_IMU.cpp to match class name (#3859)
bbd8980a20 [myRobot] Fix cameraserver library order (#3858)
831052f118 [wpilib] Add simulation support to ADIS classes (#3857)
c137569f91 [wpilib] Throw exception if the REV Pneumatic Hub firmware version is older than 22.0.0 (#3853)
dae61226fa Fix Maven Artifacts readme (#3856)
3ad4594a88 Update Maven artifacts readme for 2022 (#3855)
112acb9a62 [wpilibc] Move ADIS IMU constants to inside class (#3852)
ecee224e81 [wpilib] Allow SendableCameraWrappers to take arbitrary URLs (#3850)
a3645dea34 LICENSE: Bump year range to include 2022 (#3854)
7c09f44898 [wpilib] Use PSI for compressor config and sensor reading (#3847)
f401ea9aae [wpigui] Remove wpiutil linkage (#3851)
bf8517f1e6 [wpimath] TimeInterpolatableBufferTest: Fix lint warnings (#3849)
528087e308 [hal] Use enums with fixed underlying type in clang C (#3297)
1f59ff72f9 [wpilib] Add ADIS IMUs (#3777)
315be873c4 [wpimath] Add TimeInterpolatableBuffer (#2695)
b8d019cdb4 [wpilib] Rename NormalizeWheelSpeeds to DesaturateWheelSpeeds (#3791)
102f23bbdb [wpilibj] DriverStation: Set thread interrupted state (#3846)
b85c24a79c [wpilib] Add warning about onboard I2C (#3842)
eee29daaf9 [newCommands] Trigger: Allow override of debounce type (#3845)
aa9dfabde2 [wpimath] Move debouncer to filters (#3838)
5999a26fba [wpiutil] Add GetSystemTime() (#3840)
1e82595ffb [examples] Fix arcade inversions (#3841)
e373fa476b [wpiutil] Add disableMockTime to JNI (#3839)
dceb5364f4 [examples] Ensure right side motors are inverted (#3836)
baacbc8e24 [wpilib] Tachometer: Add function to return RPS (#3833)
84b15f0883 [templates] Add Java Romi Educational template (#3837)
c0da9d2d35 [examples] Invert Right Motor in Romi Java examples (#3828)
0fe0be2733 [build] Change project year to intellisense (#3835)
eafa947338 [wpimath] Make copies of trajectory constraint arguments (#3832)
9d13ae8d01 [wpilib] Add notes for Servo get that it only returns cmd (NFC) (#3820)
2a64e4bae5 [wpimath] Give drivetrain a more realistic width in TrajectoryJsonTest.java (#3822)
c3fd20db59 [wpilib] Fix trajectory sampling in DifferentialDriveSim test (#3821)
6f91f37cd0 [examples] Fix SwerveControllerCommand order of Module States (#3815)
5158730b81 [wpigui] Upgrade to imgui 1.86, GLFW 3.3.6 (#3817)
2ad2d2ca96 [wpiutil] MulticastServiceResolver: Fix C array returning functions (#3816)
b5fd29774f [wpilibj] Trigger: implement BooleanSupplier interface (#3811)
9f8f330e96 [wpilib] Fix Mecanum and SwerveControllerCommand when desired rotation passed (#3808)
1ad3b1b333 [hal] Don't copy byte to where null terminator goes (#3807)
dfc24425c3 [build] Fix gazebo gradle IDE warnings (#3806)
c02577bb51 [glass] Configure delay loading for windows camera server support (#3803)
c9e6a96a61 [wpilib] Document range of Servo angle (NFC) (#3796)
9778626f34 [wpilib, hal] Add support for getting faults and versions from power distribution (#3794)
34b2d0dae1 [wpilib, hal] High Level REV PH changes (#3792)
59a7528fd6 [cscore] Fix crash when usbcamera is deleted before message pump thread fully starts (#3804)
11d9859ef1 [build] Update plugins to remove log4j vulnerabilities (#3805)
e44ed752ad [glass] Fix CollapsingHeader in Encoder, PCM, and DeviceTree (#3797)
52b2dd5b89 [build] Bump native utils to remove log4j (#3802)
c46636f218 [wpilib] Improve new counter classes documentation (NFC) (#3801)
dc531462e1 [build] Update to gradle 7.3.2 (#3800)
92ba98621c [wpimath] Add helper variable templates for units type traits (#3790)
d41d051f1b [wpilibc] Fix Mecanum & Swerve ControllerCommand lambda capture (#3795)
c5ae0effac OtherVersions.md: Add one missing case of useLocal (#3788)
b3974c6ed3 [wpimath] Upgrade to Drake v0.37.0 (#3786)
589a00e379 [wpilibc] Start DriverStation thread from RobotBase (#3785)
8d9836ca02 [wpilib] Improve curvature drive documentation (NFC) (#3783)
8b5bf8632e [myRobot] Add wpimath and wpiutil JNI (#3784)
1846114491 [examples] Update references from characterization to SysId (NFC) (#3782)
2c461c794e [build] Update to gradle 7.3 (#3778)
109363daa4 [hal] Add remaining driver functions for REVPH (#3776)
41d26bee8d [hal] Refactor REV PDH (#3775)
7269a170fb Upgrade maven deps to latest versions and fix new linter errors (#3772)
441f2ed9b0 [build] actions: use fixed image versions instead latest (#3761)
15275433d4 [examples] Fix duplicate port allocations in C++ SwerveBot/SwerveDrivePoseEstimator/RomiReference (#3773)
1ac02d2f58 [examples] Fix drive Joystick axes in several examples (#3769)
8ee6257e92 [wpilib] DifferentialDrivetrainSim.KitbotMotor: Add NEO and Falcon 500 (#3762)
d81ef2bc5c [wpilib] Fix deadlocks in Mechanism2d et al. (#3770)
acb64dff97 [wpimath] Make RamseteController::Calculate() more concise (#3763)
3f6cf76a8c [hal] Refactor REV PH CAN frames (#3756)
3ef2dab465 [wpilib] DutyCycleEncoder: add setting of duty cycle range (#3759)
a5a56dd067 Readme: Add Visual Studio 2022 (#3760)
04957a6d30 [wpimath] Fix units of RamseteController's b and zeta (#3757)
5da54888f8 [glass] Upgrade imgui to 0.85, implot to HEAD, glfw to 3.3.5 (#3754)
6c93365b0f [wpiutil] MulticastService cleanup (#3750)
1c4a8bfb66 [cscore] Cleanup Windows USB camera impl (#3751)
d51a1d3b3d [rtns] Fix icon (#3749)
aced2e7da6 Add roboRIO Team Number Setter tool (#3744)
fa1ceca83a [wpilibj] Use DS cache for iterative robot control word cache (#3748)
0ea05d34e6 [build] Update to gradle 7.2 (#3746)
09db4f672b [build] Update to native utils 2022.6.1 (#3745)
4ba80a3a8c [wpigui] Don't recursively render frames in size callback (#3743)
ae208d2b17 [wpiutil] StringExtras: Add substr() (#3742)
6f51cb3b98 [wpiutil] MulticastResolver: make event manual reset, change to multiple read (#3736)
f6159ee1a2 [glass] Fix Drive widget handling of negative rotation (#3739)
7f401ae895 [build] Update NI libraries to 2022.2.3 (#3738)
0587b7043a [glass] Use JSON files for storage instead of imgui ini
0bbf51d566 [wpigui] Change maximized to bool
92c6eae6b0 [wpigui] PFD: Add explicit to constructors
141354cd79 [wpigui] Add hooks for custom load/save settings
f6e9fc7d71 [wpiutil] Handle multicast service collision on linux (#3734)
d8418be7d1 [glass, outlineviewer] Return 0 from WinMain (#3735)
82066946e5 [wpiutil] Add mDNS resolver and announcer (#3733)
4b1defc8d8 [wpilib] Remove automatic PD type from module type enum (#3732)
da90c1cd2c [wpilib] Add bang-bang controller (#3676)
3aa54fa027 [wpilib] Add new counter implementations (#2447)
b156db400d [hal, wpilib] Incorporate pneumatic control type into wpilibc/j (#3728)
9aba2b7583 [oldCommands] Add wrappers for WPILib objects to work with old PID Controller (#3710)
a9931223f0 [hal] Add REV PH faults (#3729)
aacf9442e4 [wpimath] Fix units typo in LinearSystemId source comment (#3730)
7db10ecf00 [wpilibc] Make SPI destructor virtual since SPI contains virtual functions (#3727)
a0a5b2aea5 [wpimath] Upgrade to EJML 0.41 (#3726)
eb835598a4 [hal] Add HAL functions for compressor config modes on REV PH (#3724)
f0ab6df5b6 [wpimath] Upgrade to Drake v0.36.0 (#3722)
075144faa3 [docs] Parse files without extensions with Doxygen (#3721)
32468a40cb [hal] Remove use of getDmaDescriptor from autospi (#3717)
38611e9dd7 [hal] Fix REVPH Analog Pressure channel selection (#3716)
4d78def31e [wpilib] Add DeadbandElimination forwarding to PWMMotorController (#3714)
3be0c1217a [wpilibcExamples] Make GearsBot use idiomatic C++ (#3711)
42d3a50aa2 [hal] Check error codes during serial port initialization (#3712)
52f1464029 Add project with field images and their json config files (#3668)
68ce62e2e9 [hal] Add autodetect for power modules (#3706)
3dd41c0d37 [wpilib] Don't print PD errors for LiveWindow reads (#3708)
7699a1f827 [hal] Fix sim not working with automatic PD type and default module (#3707)
Change-Id: I6b0b8fa8b2d2a24071240f624db9ec6d127f6648
git-subtree-dir: third_party/allwpilib
git-subtree-split: e4b91005cf69161f1cb3d934f6526232e6b9169e
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt
index 2a4090f..4e1cac3 100644
--- a/wpimath/CMakeLists.txt
+++ b/wpimath/CMakeLists.txt
@@ -13,26 +13,26 @@
include(UseJava)
set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
- if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.38.jar")
+ if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.41.jar")
set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
set(JAR_ROOT "${WPILIB_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")
+ file(DOWNLOAD "${BASE_URL}org/ejml/ejml-cdense/0.41/ejml-cdense-0.41.jar"
+ "${JAR_ROOT}/ejml-cdense-0.41.jar")
+ file(DOWNLOAD "${BASE_URL}org/ejml/ejml-core/0.41/ejml-core-0.41.jar"
+ "${JAR_ROOT}/ejml-core-0.41.jar")
+ file(DOWNLOAD "${BASE_URL}org/ejml/ejml-ddense/0.41/ejml-ddense-0.41.jar"
+ "${JAR_ROOT}/ejml-ddense-0.41.jar")
+ file(DOWNLOAD "${BASE_URL}org/ejml/ejml-dsparse/0.41/ejml-dsparse-0.41.jar"
+ "${JAR_ROOT}/ejml-dsparse-0.41.jar")
+ file(DOWNLOAD "${BASE_URL}org/ejml/ejml-fdense/0.41/ejml-fdense-0.41.jar"
+ "${JAR_ROOT}/ejml-fdense-0.41.jar")
+ file(DOWNLOAD "${BASE_URL}org/ejml/ejml-simple/0.41/ejml-simple-0.41.jar"
+ "${JAR_ROOT}/ejml-simple-0.41.jar")
+ file(DOWNLOAD "${BASE_URL}org/ejml/ejml-zdense/0.41/ejml-zdense-0.41.jar"
+ "${JAR_ROOT}/ejml-zdense-0.41.jar")
message(STATUS "All files downloaded.")
endif()
diff --git a/wpimath/build.gradle b/wpimath/build.gradle
index 79db4bc..c31f04c 100644
--- a/wpimath/build.gradle
+++ b/wpimath/build.gradle
@@ -32,10 +32,10 @@
}
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"
+ api "org.ejml:ejml-simple:0.41"
+ api "com.fasterxml.jackson.core:jackson-annotations:2.12.4"
+ api "com.fasterxml.jackson.core:jackson-core:2.12.4"
+ api "com.fasterxml.jackson.core:jackson-databind:2.12.4"
}
def wpilibNumberFileInput = file("src/generate/GenericNumber.java.jinja")
diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
index 791de8a..25b9f92 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
@@ -84,4 +84,17 @@
public static double angleModulus(double angleRadians) {
return inputModulus(angleRadians, -Math.PI, Math.PI);
}
+
+ /**
+ * Perform linear interpolation between two values.
+ *
+ * @param startValue The value to start at.
+ * @param endValue The value to end at.
+ * @param t How far between the two values to interpolate. This is clamped to [0, 1].
+ * @return The interpolated value.
+ */
+ @SuppressWarnings("ParameterName")
+ public static double interpolate(double startValue, double endValue, double t) {
+ return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
index 8baf401..69430eb 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
@@ -153,7 +153,7 @@
}
/**
- * Normalize all inputs if any excedes the maximum magnitude. Useful for systems such as
+ * Renormalize all inputs if any exceeds the maximum magnitude. Useful for systems such as
* differential drivetrains.
*
* @param u The input vector.
@@ -161,7 +161,7 @@
* @param <I> The number of inputs.
* @return The normalizedInput
*/
- public static <I extends Num> Matrix<I, N1> normalizeInputVector(
+ public static <I extends Num> Matrix<I, N1> desaturateInputVector(
Matrix<I, N1> u, double maxMagnitude) {
double maxValue = u.maxAbs();
boolean isCapped = maxValue > maxMagnitude;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/BangBangController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/BangBangController.java
new file mode 100644
index 0000000..7e8c778
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/BangBangController.java
@@ -0,0 +1,159 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Implements a bang-bang controller, which outputs either 0 or 1 depending on whether the
+ * measurement is less than the setpoint. This maximally-aggressive control approach works very well
+ * for velocity control of high-inertia mechanisms, and poorly on most other things.
+ *
+ * <p>Note that this is an *asymmetric* bang-bang controller - it will not exert any control effort
+ * in the reverse direction (e.g. it won't try to slow down an over-speeding shooter wheel). This
+ * asymmetry is *extremely important.* Bang-bang control is extremely simple, but also potentially
+ * hazardous. Always ensure that your motor controllers are set to "coast" before attempting to
+ * control them with a bang-bang controller.
+ */
+public class BangBangController implements Sendable {
+ private static int instances;
+
+ private double m_tolerance;
+
+ private double m_setpoint;
+ private double m_measurement;
+
+ /**
+ * Creates a new bang-bang controller.
+ *
+ * <p>Always ensure that your motor controllers are set to "coast" before attempting to control
+ * them with a bang-bang controller.
+ *
+ * @param tolerance Tolerance for {@link #atSetpoint() atSetpoint}.
+ */
+ public BangBangController(double tolerance) {
+ instances++;
+
+ setTolerance(tolerance);
+
+ SendableRegistry.addLW(this, "BangBangController", instances);
+
+ MathSharedStore.reportUsage(MathUsageId.kController_PIDController2, instances);
+ }
+
+ /**
+ * Creates a new bang-bang controller.
+ *
+ * <p>Always ensure that your motor controllers are set to "coast" before attempting to control
+ * them with a bang-bang controller.
+ */
+ public BangBangController() {
+ this(Double.POSITIVE_INFINITY);
+ }
+
+ /**
+ * Sets the setpoint for the bang-bang controller.
+ *
+ * @param setpoint The desired setpoint.
+ */
+ public void setSetpoint(double setpoint) {
+ m_setpoint = setpoint;
+ }
+
+ /**
+ * Returns the current setpoint of the bang-bang controller.
+ *
+ * @return The current setpoint.
+ */
+ public double getSetpoint() {
+ return m_setpoint;
+ }
+
+ /**
+ * Returns true if the error is within the tolerance of the setpoint.
+ *
+ * @return Whether the error is within the acceptable bounds.
+ */
+ public boolean atSetpoint() {
+ return Math.abs(m_setpoint - m_measurement) < m_tolerance;
+ }
+
+ /**
+ * Sets the error within which atSetpoint will return true.
+ *
+ * @param tolerance Position error which is tolerable.
+ */
+ public void setTolerance(double tolerance) {
+ m_tolerance = tolerance;
+ }
+
+ /**
+ * Returns the current tolerance of the controller.
+ *
+ * @return The current tolerance.
+ */
+ public double getTolerance() {
+ return m_tolerance;
+ }
+
+ /**
+ * Returns the current measurement of the process variable.
+ *
+ * @return The current measurement of the process variable.
+ */
+ public double getMeasurement() {
+ return m_measurement;
+ }
+
+ /**
+ * Returns the current error.
+ *
+ * @return The current error.
+ */
+ public double getError() {
+ return m_setpoint - m_measurement;
+ }
+
+ /**
+ * Returns the calculated control output.
+ *
+ * <p>Always ensure that your motor controllers are set to "coast" before attempting to control
+ * them with a bang-bang controller.
+ *
+ * @param measurement The most recent measurement of the process variable.
+ * @param setpoint The setpoint for the process variable.
+ * @return The calculated motor output (0 or 1).
+ */
+ public double calculate(double measurement, double setpoint) {
+ m_measurement = measurement;
+ m_setpoint = setpoint;
+
+ return measurement < setpoint ? 1 : 0;
+ }
+
+ /**
+ * Returns the calculated control output.
+ *
+ * @param measurement The most recent measurement of the process variable.
+ * @return The calculated motor output (0 or 1).
+ */
+ public double calculate(double measurement) {
+ return calculate(measurement, m_setpoint);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("BangBangController");
+ builder.addDoubleProperty("tolerance", this::getTolerance, this::setTolerance);
+ builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
+ builder.addDoubleProperty("measurement", this::getMeasurement, null);
+ builder.addDoubleProperty("error", this::getError, null);
+ builder.addBooleanProperty("atSetpoint", this::atSetpoint, null);
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
index cf4b26d..e244c5f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
@@ -104,10 +104,11 @@
if (!StateSpaceUtil.isStabilizable(discA, discB)) {
var builder = new StringBuilder("The system passed to the LQR is uncontrollable!\n\nA =\n");
- builder.append(discA.getStorage().toString());
- builder.append("\nB =\n");
- builder.append(discB.getStorage().toString());
- builder.append("\n");
+ builder
+ .append(discA.getStorage().toString())
+ .append("\nB =\n")
+ .append(discB.getStorage().toString())
+ .append('\n');
var msg = builder.toString();
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
@@ -117,8 +118,13 @@
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
- var temp = discB.transpose().times(S).times(discB).plus(R);
- m_K = temp.solve(discB.transpose().times(S).times(discA));
+ m_K =
+ discB
+ .transpose()
+ .times(S)
+ .times(discB)
+ .plus(R)
+ .solve(discB.transpose().times(S).times(discA));
m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
@@ -151,8 +157,13 @@
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
- var temp = discB.transpose().times(S).times(discB).plus(R);
- m_K = temp.solve(discB.transpose().times(S).times(discA).plus(N.transpose()));
+ m_K =
+ discB
+ .transpose()
+ .times(S)
+ .times(discB)
+ .plus(R)
+ .solve(discB.transpose().times(S).times(discA).plus(N.transpose()));
m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
index 0be592e..a1d6237 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
@@ -46,10 +46,10 @@
/**
* Construct a Ramsete unicycle controller.
*
- * @param b Tuning parameter (b > 0) for which larger values make convergence more aggressive
- * like a proportional term.
- * @param zeta Tuning parameter (0 < zeta < 1) for which larger values provide more damping
- * in response.
+ * @param b Tuning parameter (b > 0 rad²/m²) for which larger values make convergence more
+ * aggressive like a proportional term.
+ * @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger values provide
+ * more damping in response.
*/
@SuppressWarnings("ParameterName")
public RamseteController(double b, double zeta) {
@@ -58,8 +58,8 @@
}
/**
- * Construct a Ramsete unicycle controller. The default arguments for b and zeta of 2.0 and 0.7
- * have been well-tested to produce desirable results.
+ * Construct a Ramsete unicycle controller. The default arguments for b and zeta of 2.0 rad²/m²
+ * and 0.7 rad⁻¹ have been well-tested to produce desirable results.
*/
public RamseteController() {
this(2.0, 0.7);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
index f985960..a1e5f68 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
@@ -128,7 +128,7 @@
}
/**
- * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
+ * Calculates the minimum achievable acceleration given a maximum voltage supply and a velocity.
* Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
* simultaneously achievable - enter the velocity constraint, and this will give you a
* simultaneously-achievable acceleration constraint.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
index 074362e..b199d4d 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
@@ -40,8 +40,8 @@
* <p><strong> x = [x, y, theta, dist_l, dist_r]ᵀ </strong> in the field coordinate system
* containing x position, y position, heading, left encoder distance, and right encoder distance.
*
- * <p><strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity, right wheel
- * velocity, and change in gyro heading.
+ * <p><strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity, and angular rate
+ * in the field coordinate system.
*
* <p>NB: Using velocities make things considerably easier, because it means that teams don't have
* to worry about getting an accurate model. Basically, we suspect that it's easier for teams to get
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
index 4aea69d..c992d2f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
@@ -79,10 +79,11 @@
if (!StateSpaceUtil.isDetectable(discA, C)) {
var builder =
new StringBuilder("The system passed to the Kalman filter is unobservable!\n\nA =\n");
- builder.append(discA.getStorage().toString());
- builder.append("\nC =\n");
- builder.append(C.getStorage().toString());
- builder.append("\n");
+ builder
+ .append(discA.getStorage().toString())
+ .append("\nC =\n")
+ .append(C.getStorage().toString())
+ .append('\n');
var msg = builder.toString();
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
index 50a83b5..89f35e5 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
@@ -74,15 +74,18 @@
return;
}
- // This index starts at one because we use the previous state later on, and we always want to
- // have a "previous state".
- int maxIdx = m_pastObserverSnapshots.size() - 1;
- int low = 1;
- int high = Math.max(maxIdx, 1);
+ // Use a less verbose name for timestamp
+ double timestamp = timestampSeconds;
+ int maxIdx = m_pastObserverSnapshots.size() - 1;
+ int low = 0;
+ int high = maxIdx;
+
+ // Perform a binary search to find the index of first snapshot whose
+ // timestamp is greater than or equal to the global measurement timestamp
while (low != high) {
int mid = (low + high) / 2;
- if (m_pastObserverSnapshots.get(mid).getKey() < timestampSeconds) {
+ if (m_pastObserverSnapshots.get(mid).getKey() < timestamp) {
// This index and everything under it are less than the requested timestamp. Therefore, we
// can discard them.
low = mid + 1;
@@ -93,16 +96,37 @@
}
}
- // We are simply assigning this index to a new variable to avoid confusion
- // with variable names.
- int index = low;
- double timestamp = timestampSeconds;
- int indexOfClosestEntry =
- Math.abs(timestamp - m_pastObserverSnapshots.get(index - 1).getKey())
- <= Math.abs(
- timestamp - m_pastObserverSnapshots.get(Math.min(index, maxIdx)).getKey())
- ? index - 1
- : index;
+ int indexOfClosestEntry;
+
+ if (low == 0) {
+ // If the global measurement is older than any snapshot, throw out the
+ // measurement because there's no state estimate into which to incorporate
+ // the measurement
+ if (timestamp < m_pastObserverSnapshots.get(low).getKey()) {
+ return;
+ }
+
+ // If the first snapshot has same timestamp as the global measurement, use
+ // that snapshot
+ indexOfClosestEntry = 0;
+ } else if (low == maxIdx && m_pastObserverSnapshots.get(low).getKey() < timestamp) {
+ // If all snapshots are older than the global measurement, use the newest
+ // snapshot
+ indexOfClosestEntry = maxIdx;
+ } else {
+ // Index of snapshot taken after the global measurement
+ int nextIdx = low;
+
+ // Index of snapshot taken before the global measurement. Since we already
+ // handled the case where the index points to the first snapshot, this
+ // computation is guaranteed to be nonnegative.
+ int prevIdx = nextIdx - 1;
+
+ // Find the snapshot closest in time to global measurement
+ double prevTimeDiff = Math.abs(timestamp - m_pastObserverSnapshots.get(prevIdx).getKey());
+ double nextTimeDiff = Math.abs(timestamp - m_pastObserverSnapshots.get(nextIdx).getKey());
+ indexOfClosestEntry = prevTimeDiff <= nextTimeDiff ? prevIdx : nextIdx;
+ }
double lastTimestamp =
m_pastObserverSnapshots.get(indexOfClosestEntry).getKey() - nominalDtSeconds;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
index 42a6adb..d6f1075 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
@@ -38,8 +38,8 @@
* <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
* position, and heading.
*
- * <p><strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity, right wheel
- * velocity, and change in gyro heading.
+ * <p><strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity, and angular rate
+ * in the field coordinate system.
*
* <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
* heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
index 9e48728..8782b7b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
@@ -38,8 +38,8 @@
* <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
* position, and heading.
*
- * <p><strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity, right wheel
- * velocity, and change in gyro heading.
+ * <p><strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity, and angular rate
+ * in the field coordinate system.
*
* <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
* heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
new file mode 100644
index 0000000..8da45e9
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
@@ -0,0 +1,91 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.filter;
+
+import edu.wpi.first.util.WPIUtilJNI;
+
+/**
+ * A simple debounce filter for boolean streams. Requires that the boolean change value from
+ * baseline for a specified period of time before the filtered value changes.
+ */
+public class Debouncer {
+ public enum DebounceType {
+ kRising,
+ kFalling,
+ kBoth
+ }
+
+ private final double m_debounceTimeSeconds;
+ private final DebounceType m_debounceType;
+ private boolean m_baseline;
+
+ private double m_prevTimeSeconds;
+
+ /**
+ * Creates a new Debouncer.
+ *
+ * @param debounceTime The number of seconds the value must change from baseline for the filtered
+ * value to change.
+ * @param type Which type of state change the debouncing will be performed on.
+ */
+ public Debouncer(double debounceTime, DebounceType type) {
+ m_debounceTimeSeconds = debounceTime;
+ m_debounceType = type;
+
+ resetTimer();
+
+ switch (m_debounceType) {
+ case kBoth: // fall-through
+ case kRising:
+ m_baseline = false;
+ break;
+ case kFalling:
+ m_baseline = true;
+ break;
+ default:
+ throw new IllegalArgumentException("Invalid debounce type!");
+ }
+ }
+
+ /**
+ * Creates a new Debouncer. Baseline value defaulted to "false."
+ *
+ * @param debounceTime The number of seconds the value must change from baseline for the filtered
+ * value to change.
+ */
+ public Debouncer(double debounceTime) {
+ this(debounceTime, DebounceType.kRising);
+ }
+
+ private void resetTimer() {
+ m_prevTimeSeconds = WPIUtilJNI.now() * 1e-6;
+ }
+
+ private boolean hasElapsed() {
+ return (WPIUtilJNI.now() * 1e-6) - m_prevTimeSeconds >= m_debounceTimeSeconds;
+ }
+
+ /**
+ * Applies the debouncer to the input stream.
+ *
+ * @param input The current value of the input stream.
+ * @return The debounced value of the input stream.
+ */
+ public boolean calculate(boolean input) {
+ if (input == m_baseline) {
+ resetTimer();
+ }
+
+ if (hasElapsed()) {
+ if (m_debounceType == DebounceType.kBoth) {
+ m_baseline = input;
+ resetTimer();
+ }
+ return input;
+ } else {
+ return m_baseline;
+ }
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
index 93d6bea..5eac0b3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
@@ -138,42 +138,41 @@
}
/**
- * Creates a backward finite difference filter that computes the nth derivative of the input given
- * the specified number of samples.
+ * Creates a finite difference filter that computes the nth derivative of the input given the
+ * specified stencil points.
*
- * <p>For example, a first derivative filter that uses two samples and a sample period of 20 ms
- * would be
- *
- * <pre><code>
- * LinearFilter.backwardFiniteDifference(1, 2, 0.02);
- * </code></pre>
+ * <p>Stencil points are the indices of the samples to use in the finite difference. 0 is the
+ * current sample, -1 is the previous sample, -2 is the sample before that, etc. Don't use
+ * positive stencil points (samples from the future) if the LinearFilter will be used for
+ * stream-based online filtering.
*
* @param derivative The order of the derivative to compute.
* @param samples The number of samples to use to compute the given derivative. This must be one
* more than the order of derivative or higher.
+ * @param stencil List of stencil points.
* @param period The period in seconds between samples taken by the user.
* @return Linear filter.
+ * @throws IllegalArgumentException if derivative < 1, samples <= 0, or derivative >=
+ * samples.
*/
@SuppressWarnings("LocalVariableName")
- public static LinearFilter backwardFiniteDifference(int derivative, int samples, double period) {
+ public static LinearFilter finiteDifference(
+ int derivative, int samples, int[] stencil, double period) {
// See
// https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
//
- // <p>For a given list of stencil points s of length n and the order of
+ // For a given list of stencil points s of length n and the order of
// derivative d < n, the finite difference coefficients can be obtained by
// solving the following linear system for the vector a.
//
- // <pre>
// [s₁⁰ ⋯ sₙ⁰ ][a₁] [ δ₀,d ]
// [ ⋮ ⋱ ⋮ ][⋮ ] = d! [ ⋮ ]
// [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ] [δₙ₋₁,d]
- // </pre>
//
- // <p>where δᵢ,ⱼ are the Kronecker delta. For backward finite difference,
- // the stencil points are the range [-n + 1, 0]. The FIR gains are the
- // elements of the vector a in reverse order divided by hᵈ.
+ // where δᵢ,ⱼ are the Kronecker delta. The FIR gains are the elements of the
+ // vector a in reverse order divided by hᵈ.
//
- // <p>The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
+ // The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
if (derivative < 1) {
throw new IllegalArgumentException(
@@ -192,8 +191,7 @@
var S = new SimpleMatrix(samples, samples);
for (int row = 0; row < samples; ++row) {
for (int col = 0; col < samples; ++col) {
- double s = 1 - samples + col;
- S.set(row, col, Math.pow(s, row));
+ S.set(row, col, Math.pow(stencil[col], row));
}
}
@@ -211,9 +209,34 @@
ffGains[i] = a.get(samples - i - 1, 0);
}
- double[] fbGains = new double[0];
+ return new LinearFilter(ffGains, new double[0]);
+ }
- return new LinearFilter(ffGains, fbGains);
+ /**
+ * Creates a backward finite difference filter that computes the nth derivative of the input given
+ * the specified number of samples.
+ *
+ * <p>For example, a first derivative filter that uses two samples and a sample period of 20 ms
+ * would be
+ *
+ * <pre><code>
+ * LinearFilter.backwardFiniteDifference(1, 2, 0.02);
+ * </code></pre>
+ *
+ * @param derivative The order of the derivative to compute.
+ * @param samples The number of samples to use to compute the given derivative. This must be one
+ * more than the order of derivative or higher.
+ * @param period The period in seconds between samples taken by the user.
+ * @return Linear filter.
+ */
+ public static LinearFilter backwardFiniteDifference(int derivative, int samples, double period) {
+ // Generate stencil points from -(samples - 1) to 0
+ int[] stencil = new int[samples];
+ for (int i = 0; i < samples; ++i) {
+ stencil[i] = -(samples - 1) + i;
+ }
+
+ return finiteDifference(derivative, samples, stencil, period);
}
/** Reset the filter state. */
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
index 6033b89..f64eff4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
@@ -8,12 +8,13 @@
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
/** Represents a 2d pose containing translational and rotational elements. */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Pose2d {
+public class Pose2d implements Interpolatable<Pose2d> {
private final Translation2d m_translation;
private final Rotation2d m_rotation;
@@ -242,4 +243,18 @@
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
+
+ @Override
+ @SuppressWarnings("ParameterName")
+ public Pose2d interpolate(Pose2d endValue, double t) {
+ if (t < 0) {
+ return this;
+ } else if (t >= 1) {
+ return endValue;
+ } else {
+ var twist = this.log(endValue);
+ var scaledTwist = new Twist2d(twist.dx * t, twist.dy * t, twist.dtheta * t);
+ return this.exp(scaledTwist);
+ }
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
index 74ef228..08e5438 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
@@ -8,12 +8,14 @@
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
/** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Rotation2d {
+public class Rotation2d implements Interpolatable<Rotation2d> {
private final double m_value;
private final double m_cos;
private final double m_sin;
@@ -198,4 +200,10 @@
public int hashCode() {
return Objects.hash(m_value);
}
+
+ @Override
+ @SuppressWarnings("ParameterName")
+ public Rotation2d interpolate(Rotation2d endValue, double t) {
+ return new Rotation2d(MathUtil.interpolate(this.getRadians(), endValue.getRadians(), t));
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
index 251c078..84eaea8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
@@ -8,6 +8,8 @@
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
/**
@@ -20,7 +22,7 @@
@SuppressWarnings({"ParameterName", "MemberName"})
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Translation2d {
+public class Translation2d implements Interpolatable<Translation2d> {
private final double m_x;
private final double m_y;
@@ -196,4 +198,11 @@
public int hashCode() {
return Objects.hash(m_x, m_y);
}
+
+ @Override
+ public Translation2d interpolate(Translation2d endValue, double t) {
+ return new Translation2d(
+ MathUtil.interpolate(this.getX(), endValue.getX(), t),
+ MathUtil.interpolate(this.getY(), endValue.getY(), t));
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java
new file mode 100644
index 0000000..f10820f
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+/**
+ * An object should extend interpolatable if you wish to interpolate between a lower and upper
+ * bound, such as a robot position on the field between timesteps. This behavior can be linear or
+ * nonlinear.
+ *
+ * @param <T> The class that is interpolatable.
+ */
+public interface Interpolatable<T> {
+ /**
+ * Return the interpolated value. This object is assumed to be the starting position, or lower
+ * bound.
+ *
+ * @param endValue The upper bound, or end.
+ * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
+ * @return The interpolated value.
+ */
+ @SuppressWarnings("ParameterName")
+ T interpolate(T endValue, double t);
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
new file mode 100644
index 0000000..676bd7c
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
@@ -0,0 +1,149 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+import java.util.NavigableMap;
+import java.util.TreeMap;
+
+/**
+ * The TimeInterpolatableBuffer provides an easy way to estimate past measurements. One application
+ * might be in conjunction with the DifferentialDrivePoseEstimator, where knowledge of the robot
+ * pose at the time when vision or other global measurement were recorded is necessary, or for
+ * recording the past angles of mechanisms as measured by encoders.
+ *
+ * @param <T> The type stored in this buffer.
+ */
+public class TimeInterpolatableBuffer<T> {
+ private final double m_historySize;
+ private final InterpolateFunction<T> m_interpolatingFunc;
+ private final NavigableMap<Double, T> m_buffer = new TreeMap<>();
+
+ private TimeInterpolatableBuffer(
+ InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+ this.m_historySize = historySizeSeconds;
+ this.m_interpolatingFunc = interpolateFunction;
+ }
+
+ /**
+ * Create a new TimeInterpolatableBuffer.
+ *
+ * @param interpolateFunction The function used to interpolate between values.
+ * @param historySizeSeconds The history size of the buffer.
+ * @param <T> The type of data to store in the buffer.
+ * @return The new TimeInterpolatableBuffer.
+ */
+ public static <T> TimeInterpolatableBuffer<T> createBuffer(
+ InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+ return new TimeInterpolatableBuffer<>(interpolateFunction, historySizeSeconds);
+ }
+
+ /**
+ * Create a new TimeInterpolatableBuffer that stores a given subclass of {@link Interpolatable}.
+ *
+ * @param historySizeSeconds The history size of the buffer.
+ * @param <T> The type of {@link Interpolatable} to store in the buffer.
+ * @return The new TimeInterpolatableBuffer.
+ */
+ public static <T extends Interpolatable<T>> TimeInterpolatableBuffer<T> createBuffer(
+ double historySizeSeconds) {
+ return new TimeInterpolatableBuffer<>(Interpolatable::interpolate, historySizeSeconds);
+ }
+
+ /**
+ * Create a new TimeInterpolatableBuffer to store Double values.
+ *
+ * @param historySizeSeconds The history size of the buffer.
+ * @return The new TimeInterpolatableBuffer.
+ */
+ public static TimeInterpolatableBuffer<Double> createDoubleBuffer(double historySizeSeconds) {
+ return new TimeInterpolatableBuffer<>(MathUtil::interpolate, historySizeSeconds);
+ }
+
+ /**
+ * Add a sample to the buffer.
+ *
+ * @param timeSeconds The timestamp of the sample.
+ * @param sample The sample object.
+ */
+ public void addSample(double timeSeconds, T sample) {
+ cleanUp(timeSeconds);
+ m_buffer.put(timeSeconds, sample);
+ }
+
+ /**
+ * Removes samples older than our current history size.
+ *
+ * @param time The current timestamp.
+ */
+ private void cleanUp(double time) {
+ while (!m_buffer.isEmpty()) {
+ var entry = m_buffer.firstEntry();
+ if (time - entry.getKey() >= m_historySize) {
+ m_buffer.remove(entry.getKey());
+ } else {
+ return;
+ }
+ }
+ }
+
+ /** Clear all old samples. */
+ public void clear() {
+ m_buffer.clear();
+ }
+
+ /**
+ * Sample the buffer at the given time. If the buffer is empty, this will return null.
+ *
+ * @param timeSeconds The time at which to sample.
+ * @return The interpolated value at that timestamp. Might be null.
+ */
+ @SuppressWarnings("UnnecessaryParentheses")
+ public T getSample(double timeSeconds) {
+ if (m_buffer.isEmpty()) {
+ return null;
+ }
+
+ // Special case for when the requested time is the same as a sample
+ var nowEntry = m_buffer.get(timeSeconds);
+ if (nowEntry != null) {
+ return nowEntry;
+ }
+
+ var topBound = m_buffer.ceilingEntry(timeSeconds);
+ var bottomBound = m_buffer.floorEntry(timeSeconds);
+
+ // Return null if neither sample exists, and the opposite bound if the other is null
+ if (topBound == null && bottomBound == null) {
+ return null;
+ } else if (topBound == null) {
+ return bottomBound.getValue();
+ } else if (bottomBound == null) {
+ return topBound.getValue();
+ } else {
+ // Otherwise, interpolate. Because T is between [0, 1], we want the ratio of (the difference
+ // between the current time and bottom bound) and (the difference between top and bottom
+ // bounds).
+ return m_interpolatingFunc.interpolate(
+ bottomBound.getValue(),
+ topBound.getValue(),
+ ((timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
+ }
+ }
+
+ public interface InterpolateFunction<T> {
+ /**
+ * Return the interpolated value. This object is assumed to be the starting position, or lower
+ * bound.
+ *
+ * @param start The lower bound, or start.
+ * @param end The upper bound, or end.
+ * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
+ * @return The interpolated value.
+ */
+ @SuppressWarnings("ParameterName")
+ T interpolate(T start, T end, double t);
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
index b84eeba..20a38a9 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
@@ -28,15 +28,16 @@
}
/**
- * Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
- * kinematics, the requested speed from a/several modules may be above the max attainable speed
- * for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
- * speeds to make sure that all requested module speeds are below the absolute threshold, while
- * maintaining the ratio of speeds between modules.
+ * Renormalizes the wheel speeds if any either side is above the specified maximum.
+ *
+ * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
+ * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
+ * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
+ * absolute threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
*/
- public void normalize(double attainableMaxSpeedMetersPerSecond) {
+ public void desaturate(double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
index 3ea39e5..0807aba 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
@@ -86,7 +86,7 @@
* component, the robot will rotate around that corner.
* @return The wheel speeds. Use caution because they are not normalized. Sometimes, a user input
* may cause one of the wheel speeds to go above the attainable max velocity. Use the {@link
- * MecanumDriveWheelSpeeds#normalize(double)} function to rectify this issue.
+ * MecanumDriveWheelSpeeds#desaturate(double)} function to rectify this issue.
*/
public MecanumDriveWheelSpeeds toWheelSpeeds(
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
index 7a159fe..ef354ef 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
@@ -43,15 +43,16 @@
}
/**
- * Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
- * kinematics, the requested speed from a/several modules may be above the max attainable speed
- * for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
- * speeds to make sure that all requested module speeds are below the absolute threshold, while
- * maintaining the ratio of speeds between modules.
+ * Renormalizes the wheel speeds if any individual speed is above the specified maximum.
+ *
+ * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
+ * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
+ * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
+ * absolute threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
*/
- public void normalize(double attainableMaxSpeedMetersPerSecond) {
+ public void desaturate(double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed =
DoubleStream.of(
frontLeftMetersPerSecond,
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
index 4c6d9cc..dc1b9e4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
@@ -80,8 +80,8 @@
* component, the robot will rotate around that corner.
* @return An array containing the module states. Use caution because these module states are not
* normalized. Sometimes, a user input may cause one of the module speeds to go above the
- * attainable max velocity. Use the {@link #normalizeWheelSpeeds(SwerveModuleState[], double)
- * normalizeWheelSpeeds} function to rectify this issue.
+ * attainable max velocity. Use the {@link #desaturateWheelSpeeds(SwerveModuleState[], double)
+ * DesaturateWheelSpeeds} function to rectify this issue.
*/
@SuppressWarnings("LocalVariableName")
public SwerveModuleState[] toSwerveModuleStates(
@@ -171,17 +171,18 @@
}
/**
- * Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
- * kinematics, the requested speed from a/several modules may be above the max attainable speed
- * for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
- * speeds to make sure that all requested module speeds are below the absolute threshold, while
- * maintaining the ratio of speeds between modules.
+ * Renormalizes the wheel speeds if any individual speed is above the specified maximum.
+ *
+ * <p>Sometimes, after inverse kinematics, the requested speed from one or more modules may be
+ * above the max attainable speed for the driving motor on that module. To fix this issue, one can
+ * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
+ * absolute threshold, while maintaining the ratio of speeds between modules.
*
* @param moduleStates Reference to array of module states. The array will be mutated with the
* normalized speeds!
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach.
*/
- public static void normalizeWheelSpeeds(
+ public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
index 8f3da6a..dcd4e58 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
@@ -57,7 +57,7 @@
controller,
new LinearPlantInversionFeedforward<>(plant, dtSeconds),
observer,
- u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
+ u -> StateSpaceUtil.desaturateInputVector(u, maxVoltageVolts));
}
/**
@@ -104,7 +104,7 @@
controller,
feedforward,
observer,
- u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
+ u -> StateSpaceUtil.desaturateInputVector(u, maxVoltageVolts));
}
/**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
index 2933e93..1a05eb6 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
@@ -62,7 +62,7 @@
* Create a state-space model of a flywheel system. The states of the system are [angular
* velocity], inputs are [voltage], and outputs are [angular velocity].
*
- * @param motor The motor (or gearbox) attached to the arm.
+ * @param motor The motor (or gearbox) attached to the flywheel.
* @param jKgMetersSquared The moment of inertia J of the flywheel.
* @param G The reduction between motor and drum, as a ratio of output to input.
* @return A LinearSystem representing the given characterized constants.
@@ -90,6 +90,42 @@
}
/**
+ * Create a state-space model of a DC motor system. The states of the system are [angular
+ * position, angular velocity], inputs are [voltage], and outputs are [angular position, angular
+ * velocity].
+ *
+ * @param motor The motor (or gearbox) attached to the DC motor.
+ * @param jKgMetersSquared The moment of inertia J of the DC motor.
+ * @param G The reduction between motor and drum, as a ratio of output to input.
+ * @return A LinearSystem representing the given characterized constants.
+ * @throws IllegalArgumentException if jKgMetersSquared <= 0 or G <= 0.
+ */
+ @SuppressWarnings("ParameterName")
+ public static LinearSystem<N2, N1, N2> createDCMotorSystem(
+ DCMotor motor, double jKgMetersSquared, double G) {
+ if (jKgMetersSquared <= 0.0) {
+ throw new IllegalArgumentException("J must be greater than zero.");
+ }
+ if (G <= 0.0) {
+ throw new IllegalArgumentException("G must be greater than zero.");
+ }
+
+ return new LinearSystem<>(
+ Matrix.mat(Nat.N2(), Nat.N2())
+ .fill(
+ 0,
+ 1,
+ 0,
+ -G
+ * G
+ * motor.KtNMPerAmp
+ / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)),
+ VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)),
+ Matrix.eye(Nat.N2()),
+ new Matrix<>(Nat.N2(), Nat.N1()));
+ }
+
+ /**
* Create a state-space model of a differential drive drivetrain. In this model, the states are
* [v_left, v_right]ᵀ, inputs are [V_left, V_right]ᵀ and outputs are [v_left, v_right]ᵀ.
*
@@ -324,7 +360,7 @@
// omega = 2/trackwidth v
//
// So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
- // to V/m/s).
+ // to V/(m/s).
return identifyDrivetrainSystem(
kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth);
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
index 35745b5..d41a8eb 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
@@ -209,8 +209,6 @@
endAccel = Math.max(endAccel, 0);
endFullSpeed = Math.max(endFullSpeed, 0);
- double endDeccel = m_endDeccel - endAccel - endFullSpeed;
- endDeccel = Math.max(endDeccel, 0);
final double acceleration = m_constraints.maxAcceleration;
final double decceleration = -m_constraints.maxAcceleration;
@@ -229,11 +227,8 @@
deccelVelocity = velocity;
}
- double deccelDist = deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
-
- deccelDist = Math.max(deccelDist, 0);
-
double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+ double deccelDist;
if (accelDist > distToTarget) {
accelDist = distToTarget;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
index 37d8d68..2164435 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
@@ -48,7 +48,7 @@
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
- wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
+ wheelSpeeds.desaturate(m_maxSpeedMetersPerSecond);
// Return the new linear chassis speed.
return m_kinematics.toChassisSpeeds(wheelSpeeds).vxMetersPerSecond;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java
index b26cdcf..dab3699 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java
@@ -53,7 +53,7 @@
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
- wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
+ wheelSpeeds.desaturate(m_maxSpeedMetersPerSecond);
// Convert normalized wheel speeds back to chassis speeds
var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
index 5d95290..5c2e1f5 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
@@ -53,7 +53,7 @@
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
- SwerveDriveKinematics.normalizeWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond);
+ SwerveDriveKinematics.desaturateWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond);
// Convert normalized wheel speeds back to chassis speeds
var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/util/Units.java b/wpimath/src/main/java/edu/wpi/first/math/util/Units.java
index 13adcd8..017f4d0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/util/Units.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/util/Units.java
@@ -78,6 +78,46 @@
}
/**
+ * Converts given radians to rotations.
+ *
+ * @param radians The radians to convert.
+ * @return rotations Converted from radians.
+ */
+ public static double radiansToRotations(double radians) {
+ return radians / (Math.PI * 2);
+ }
+
+ /**
+ * Converts given degrees to rotations.
+ *
+ * @param degrees The degrees to convert.
+ * @return rotations Converted from radians.
+ */
+ public static double degreesToRotations(double degrees) {
+ return degrees / 360;
+ }
+
+ /**
+ * Converts given rotations to degrees.
+ *
+ * @param rotations The rotations to convert.
+ * @return degrees Converted from rotations.
+ */
+ public static double rotationsToDegrees(double rotations) {
+ return rotations * 360;
+ }
+
+ /**
+ * Converts given rotations to radians.
+ *
+ * @param rotations The rotations to convert.
+ * @return radians Converted from rotations.
+ */
+ public static double rotationsToRadians(double rotations) {
+ return rotations * 2 * Math.PI;
+ }
+
+ /**
* Converts rotations per minute to radians per second.
*
* @param rpm The rotations per minute to convert to radians per second.
diff --git a/wpimath/src/main/native/cpp/controller/BangBangController.cpp b/wpimath/src/main/native/cpp/controller/BangBangController.cpp
new file mode 100644
index 0000000..a4fda0a
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/BangBangController.cpp
@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "../../include/frc/controller/BangBangController.h"
+
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "wpimath/MathShared.h"
+
+using namespace frc;
+
+BangBangController::BangBangController(double tolerance)
+ : m_tolerance(tolerance) {
+ static int instances = 0;
+ instances++;
+}
+
+void BangBangController::SetSetpoint(double setpoint) {
+ m_setpoint = setpoint;
+}
+
+double BangBangController::GetSetpoint() const {
+ return m_setpoint;
+}
+
+bool BangBangController::AtSetpoint() const {
+ return std::abs(m_setpoint - m_measurement) < m_tolerance;
+}
+
+void BangBangController::SetTolerance(double tolerance) {
+ m_tolerance = tolerance;
+}
+
+double BangBangController::GetTolerance() const {
+ return m_tolerance;
+}
+
+double BangBangController::GetMeasurement() const {
+ return m_measurement;
+}
+
+double BangBangController::GetError() const {
+ return m_setpoint - m_measurement;
+}
+
+double BangBangController::Calculate(double measurement, double setpoint) {
+ m_measurement = measurement;
+ m_setpoint = setpoint;
+
+ return measurement < setpoint ? 1 : 0;
+}
+
+double BangBangController::Calculate(double measurement) {
+ return Calculate(measurement, m_setpoint);
+}
+
+void BangBangController::InitSendable(wpi::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("BangBangController");
+ builder.AddDoubleProperty(
+ "tolerance", [this] { return GetTolerance(); },
+ [this](double tolerance) { SetTolerance(tolerance); });
+ builder.AddDoubleProperty(
+ "setpoint", [this] { return GetSetpoint(); },
+ [this](double setpoint) { SetSetpoint(setpoint); });
+ builder.AddDoubleProperty(
+ "measurement", [this] { return GetMeasurement(); }, nullptr);
+ builder.AddDoubleProperty(
+ "error", [this] { return GetError(); }, nullptr);
+ builder.AddBooleanProperty(
+ "atSetpoint", [this] { return AtSetpoint(); }, nullptr);
+}
diff --git a/wpimath/src/main/native/cpp/controller/RamseteController.cpp b/wpimath/src/main/native/cpp/controller/RamseteController.cpp
index 8aa16d8..0fea864 100644
--- a/wpimath/src/main/native/cpp/controller/RamseteController.cpp
+++ b/wpimath/src/main/native/cpp/controller/RamseteController.cpp
@@ -6,6 +6,7 @@
#include <cmath>
+#include "units/angle.h"
#include "units/math.h"
using namespace frc;
@@ -15,17 +16,26 @@
*
* @param x Value of which to take sinc(x).
*/
-static double Sinc(double x) {
- if (std::abs(x) < 1e-9) {
- return 1.0 - 1.0 / 6.0 * x * x;
+static decltype(1 / 1_rad) Sinc(units::radian_t x) {
+ if (units::math::abs(x) < 1e-9_rad) {
+ return decltype(1 / 1_rad){1.0 - 1.0 / 6.0 * x.value() * x.value()};
} else {
- return std::sin(x) / x;
+ return units::math::sin(x) / x;
}
}
RamseteController::RamseteController(double b, double zeta)
+ : RamseteController(units::unit_t<b_unit>{b},
+ units::unit_t<zeta_unit>{zeta}) {}
+
+RamseteController::RamseteController(units::unit_t<b_unit> b,
+ units::unit_t<zeta_unit> zeta)
: m_b{b}, m_zeta{zeta} {}
+RamseteController::RamseteController()
+ : RamseteController(units::unit_t<b_unit>{2.0},
+ units::unit_t<zeta_unit>{0.7}) {}
+
bool RamseteController::AtReference() const {
const auto& eTranslate = m_poseError.Translation();
const auto& eRotate = m_poseError.Rotation();
@@ -51,19 +61,18 @@
m_poseError = poseRef.RelativeTo(currentPose);
// Aliases for equation readability
- double eX = m_poseError.X().value();
- double eY = m_poseError.Y().value();
- double eTheta = m_poseError.Rotation().Radians().value();
- double vRef = linearVelocityRef.value();
- double omegaRef = angularVelocityRef.value();
+ const auto& eX = m_poseError.X();
+ const auto& eY = m_poseError.Y();
+ const auto& eTheta = m_poseError.Rotation().Radians();
+ const auto& vRef = linearVelocityRef;
+ const auto& omegaRef = angularVelocityRef;
- double k =
- 2.0 * m_zeta * std::sqrt(std::pow(omegaRef, 2) + m_b * std::pow(vRef, 2));
+ auto k = 2.0 * m_zeta *
+ units::math::sqrt(units::math::pow<2>(omegaRef) +
+ m_b * units::math::pow<2>(vRef));
- units::meters_per_second_t v{vRef * m_poseError.Rotation().Cos() + k * eX};
- units::radians_per_second_t omega{omegaRef + k * eTheta +
- m_b * vRef * Sinc(eTheta) * eY};
- return ChassisSpeeds{v, 0_mps, omega};
+ return ChassisSpeeds{vRef * m_poseError.Rotation().Cos() + k * eX, 0_mps,
+ omegaRef + k * eTheta + m_b * vRef * Sinc(eTheta) * eY};
}
ChassisSpeeds RamseteController::Calculate(
diff --git a/wpimath/src/main/native/cpp/filter/Debouncer.cpp b/wpimath/src/main/native/cpp/filter/Debouncer.cpp
new file mode 100644
index 0000000..4e909a2
--- /dev/null
+++ b/wpimath/src/main/native/cpp/filter/Debouncer.cpp
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/filter/Debouncer.h"
+
+using namespace frc;
+
+Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
+ : m_debounceTime(debounceTime), m_debounceType(type) {
+ switch (type) {
+ case DebounceType::kBoth: // fall-through
+ case DebounceType::kRising:
+ m_baseline = false;
+ break;
+ case DebounceType::kFalling:
+ m_baseline = true;
+ break;
+ }
+ ResetTimer();
+}
+
+void Debouncer::ResetTimer() {
+ m_prevTime = units::microsecond_t(wpi::Now());
+}
+
+bool Debouncer::HasElapsed() const {
+ return units::microsecond_t(wpi::Now()) - m_prevTime >= m_debounceTime;
+}
+
+bool Debouncer::Calculate(bool input) {
+ if (input == m_baseline) {
+ ResetTimer();
+ }
+
+ if (HasElapsed()) {
+ if (m_debounceType == DebounceType::kBoth) {
+ m_baseline = input;
+ ResetTimer();
+ }
+ return input;
+ } else {
+ return m_baseline;
+ }
+}
diff --git a/wpimath/src/main/native/cpp/interpolation/TimeInterpolatableBuffer.cpp b/wpimath/src/main/native/cpp/interpolation/TimeInterpolatableBuffer.cpp
new file mode 100644
index 0000000..f594cd6
--- /dev/null
+++ b/wpimath/src/main/native/cpp/interpolation/TimeInterpolatableBuffer.cpp
@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
+
+namespace frc {
+
+// Template specialization to ensure that Pose2d uses pose exponential
+template <>
+TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
+ units::second_t historySize)
+ : m_historySize(historySize),
+ m_interpolatingFunc([](const Pose2d& start, const Pose2d& end, double t) {
+ if (t < 0) {
+ return start;
+ } else if (t >= 1) {
+ return end;
+ } else {
+ Twist2d twist = start.Log(end);
+ Twist2d scaledTwist = twist * t;
+ return start.Exp(scaledTwist);
+ }
+ }) {}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
index 42d3018..33c390d 100644
--- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
@@ -8,7 +8,7 @@
using namespace frc;
-void DifferentialDriveWheelSpeeds::Normalize(
+void DifferentialDriveWheelSpeeds::Desaturate(
units::meters_per_second_t attainableMaxSpeed) {
auto realMaxSpeed =
units::math::max(units::math::abs(left), units::math::abs(right));
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
index fc47461..dd30263 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
@@ -12,7 +12,7 @@
using namespace frc;
-void MecanumDriveWheelSpeeds::Normalize(
+void MecanumDriveWheelSpeeds::Desaturate(
units::meters_per_second_t attainableMaxSpeed) {
std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
rearLeft, rearRight};
diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
index db419f7..ecb36c2 100644
--- a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -6,6 +6,7 @@
#include <algorithm>
+#include <wpi/MathExtras.h>
#include <wpi/json.h>
#include "units/math.h"
@@ -25,7 +26,7 @@
Trajectory::State Trajectory::State::Interpolate(State endValue,
double i) const {
// Find the new [t] value.
- const auto newT = Lerp(t, endValue.t, i);
+ const auto newT = wpi::Lerp(t, endValue.t, i);
// Find the delta time between the current state and the interpolated state.
const auto deltaT = newT - t;
@@ -58,8 +59,8 @@
newS / endValue.pose.Translation().Distance(pose.Translation());
return {newT, newV, acceleration,
- Lerp(pose, endValue.pose, interpolationFrac),
- Lerp(curvature, endValue.curvature, interpolationFrac)};
+ wpi::Lerp(pose, endValue.pose, interpolationFrac),
+ wpi::Lerp(curvature, endValue.curvature, interpolationFrac)};
}
Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
index 2e2771d..1884758 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
@@ -141,3 +141,8 @@
config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
config.IsReversed());
}
+
+void TrajectoryGenerator::SetErrorHandler(
+ std::function<void(const char*)> func) {
+ s_errorFunc = std::move(func);
+}
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
index d1c2f6f..2a308db 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
@@ -16,7 +16,7 @@
units::meters_per_second_t velocity) const {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
- wheelSpeeds.Normalize(m_maxSpeed);
+ wheelSpeeds.Desaturate(m_maxSpeed);
return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx;
}
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
index 418a904..391b99f 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
@@ -20,7 +20,7 @@
auto yVelocity = velocity * pose.Rotation().Sin();
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature});
- wheelSpeeds.Normalize(m_maxSpeed);
+ wheelSpeeds.Desaturate(m_maxSpeed);
auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
diff --git a/wpimath/src/main/native/include/drake/common/drake_assert.h b/wpimath/src/main/native/include/drake/common/drake_assert.h
index 88587fa..e9c3aa2 100644
--- a/wpimath/src/main/native/include/drake/common/drake_assert.h
+++ b/wpimath/src/main/native/include/drake/common/drake_assert.h
@@ -88,14 +88,6 @@
// Report an assertion failure; will either Abort(...) or throw.
[[noreturn]] void AssertionFailed(const char* condition, const char* func,
const char* file, int line);
-template <bool>
-constexpr void DrakeAssertWasUsedWithRawPointer() {}
-template<>
-[[deprecated("\nDRAKE DEPRECATED: When using DRAKE_ASSERT or DRAKE_DEMAND on"
-" a raw pointer, always write out DRAKE_ASSERT(foo != nullptr), do not write"
-" DRAKE_ASSERT(foo) and rely on implicit pointer-to-bool conversion."
-"\nThe deprecated code will be removed from Drake on or after 2021-12-01.")]]
-constexpr void DrakeAssertWasUsedWithRawPointer<true>() {}
} // namespace internal
namespace assert {
// Allows for specialization of how to bool-convert Conditions used in
@@ -122,8 +114,11 @@
typedef ::drake::assert::ConditionTraits< \
typename std::remove_cv_t<decltype(condition)>> Trait; \
static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
- ::drake::internal::DrakeAssertWasUsedWithRawPointer< \
- std::is_pointer_v<decltype(condition)>>(); \
+ static_assert( \
+ !std::is_pointer_v<decltype(condition)>, \
+ "When using DRAKE_DEMAND on a raw pointer, always write out " \
+ "DRAKE_DEMAND(foo != nullptr), do not write DRAKE_DEMAND(foo) " \
+ "and rely on implicit pointer-to-bool conversion."); \
if (!Trait::Evaluate(condition)) { \
::drake::internal::AssertionFailed( \
#condition, __func__, __FILE__, __LINE__); \
@@ -149,13 +144,15 @@
constexpr bool kDrakeAssertIsArmed = false;
constexpr bool kDrakeAssertIsDisarmed = true;
} // namespace drake
-# define DRAKE_ASSERT(condition) do { \
- static_assert( \
- ::drake::assert::ConditionTraits< \
- typename std::remove_cv_t<decltype(condition)>>::is_valid, \
- "Condition should be bool-convertible."); \
- ::drake::internal::DrakeAssertWasUsedWithRawPointer< \
- std::is_pointer_v<decltype(condition)>>(); \
+# define DRAKE_ASSERT(condition) do { \
+ typedef ::drake::assert::ConditionTraits< \
+ typename std::remove_cv_t<decltype(condition)>> Trait; \
+ static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
+ static_assert( \
+ !std::is_pointer_v<decltype(condition)>, \
+ "When using DRAKE_ASSERT on a raw pointer, always write out " \
+ "DRAKE_ASSERT(foo != nullptr), do not write DRAKE_ASSERT(foo) " \
+ "and rely on implicit pointer-to-bool conversion."); \
} while (0)
# define DRAKE_ASSERT_VOID(expression) static_assert( \
std::is_convertible_v<decltype(expression), void>, \
diff --git a/wpimath/src/main/native/include/drake/common/drake_throw.h b/wpimath/src/main/native/include/drake/common/drake_throw.h
index bb4bae8..fdd07a2 100644
--- a/wpimath/src/main/native/include/drake/common/drake_throw.h
+++ b/wpimath/src/main/native/include/drake/common/drake_throw.h
@@ -14,16 +14,6 @@
// Throw an error message.
[[noreturn]] void Throw(const char* condition, const char* func,
const char* file, int line);
-
-template <bool>
-constexpr void DrakeThrowUnlessWasUsedWithRawPointer() {}
-template<>
-[[deprecated("\nDRAKE DEPRECATED: When using DRAKE_THROW_UNLESS on a raw"
-" pointer, always write out DRAKE_THROW_UNLESS(foo != nullptr), do not write"
-" DRAKE_THROW_UNLESS(foo) and rely on implicit pointer-to-bool conversion."
-"\nThe deprecated code will be removed from Drake on or after 2021-12-01.")]]
-constexpr void DrakeThrowUnlessWasUsedWithRawPointer<true>() {}
-
} // namespace internal
} // namespace drake
@@ -41,14 +31,17 @@
/// users, we should err on the side of extra detail about the failure. The
/// meaning of "foo" isolated within error message text does not make it
/// clear that a null pointer is the proximate cause of the problem.
-#define DRAKE_THROW_UNLESS(condition) \
- do { \
- typedef ::drake::assert::ConditionTraits< \
- typename std::remove_cv_t<decltype(condition)>> Trait; \
- static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
- ::drake::internal::DrakeThrowUnlessWasUsedWithRawPointer< \
- std::is_pointer_v<decltype(condition)>>(); \
- if (!Trait::Evaluate(condition)) { \
- ::drake::internal::Throw(#condition, __func__, __FILE__, __LINE__); \
- } \
+#define DRAKE_THROW_UNLESS(condition) \
+ do { \
+ typedef ::drake::assert::ConditionTraits< \
+ typename std::remove_cv_t<decltype(condition)>> Trait; \
+ static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
+ static_assert( \
+ !std::is_pointer_v<decltype(condition)>, \
+ "When using DRAKE_THROW_UNLESS on a raw pointer, always write out " \
+ "DRAKE_THROW_UNLESS(foo != nullptr), do not write DRAKE_THROW_UNLESS" \
+ "(foo) and rely on implicit pointer-to-bool conversion."); \
+ if (!Trait::Evaluate(condition)) { \
+ ::drake::internal::Throw(#condition, __func__, __FILE__, __LINE__); \
+ } \
} while (0)
diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
index 730c4b9..5fdd18c 100644
--- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h
+++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
@@ -336,8 +336,8 @@
}
/**
- * Normalize all inputs if any excedes the maximum magnitude. Useful for systems
- * such as differential drivetrains.
+ * Renormalize all inputs if any exceeds the maximum magnitude. Useful for
+ * systems such as differential drivetrains.
*
* @tparam Inputs The number of inputs.
* @param u The input vector.
@@ -345,7 +345,7 @@
* @return The normalizedInput
*/
template <int Inputs>
-Eigen::Vector<double, Inputs> NormalizeInputVector(
+Eigen::Vector<double, Inputs> DesaturateInputVector(
const Eigen::Vector<double, Inputs>& u, double maxMagnitude) {
double maxValue = u.template lpNorm<Eigen::Infinity>();
diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
index eb7cb76..1f0e407 100644
--- a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
@@ -35,14 +35,14 @@
* Creates a new ArmFeedforward with the specified gains.
*
* @param kS The static gain, in volts.
- * @param kCos The gravity gain, in volts.
+ * @param kG The gravity gain, in volts.
* @param kV The velocity gain, in volt seconds per radian.
* @param kA The acceleration gain, in volt seconds^2 per radian.
*/
constexpr ArmFeedforward(
- units::volt_t kS, units::volt_t kCos, units::unit_t<kv_unit> kV,
+ units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
- : kS(kS), kCos(kCos), kV(kV), kA(kA) {}
+ : kS(kS), kG(kG), kV(kV), kA(kA) {}
/**
* Calculates the feedforward from the gains and setpoints.
@@ -56,7 +56,7 @@
units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration =
units::unit_t<Acceleration>(0)) const {
- return kS * wpi::sgn(velocity) + kCos * units::math::cos(angle) +
+ return kS * wpi::sgn(velocity) + kG * units::math::cos(angle) +
kV * velocity + kA * acceleration;
}
@@ -79,7 +79,7 @@
units::volt_t maxVoltage, units::unit_t<Angle> angle,
units::unit_t<Acceleration> acceleration) {
// Assume max velocity is positive
- return (maxVoltage - kS - kCos * units::math::cos(angle) -
+ return (maxVoltage - kS - kG * units::math::cos(angle) -
kA * acceleration) /
kV;
}
@@ -100,7 +100,7 @@
units::volt_t maxVoltage, units::unit_t<Angle> angle,
units::unit_t<Acceleration> acceleration) {
// Assume min velocity is negative, ks flips sign
- return (-maxVoltage + kS - kCos * units::math::cos(angle) -
+ return (-maxVoltage + kS - kG * units::math::cos(angle) -
kA * acceleration) /
kV;
}
@@ -121,7 +121,7 @@
units::volt_t maxVoltage, units::unit_t<Angle> angle,
units::unit_t<Velocity> velocity) {
return (maxVoltage - kS * wpi::sgn(velocity) -
- kCos * units::math::cos(angle) - kV * velocity) /
+ kG * units::math::cos(angle) - kV * velocity) /
kA;
}
@@ -144,7 +144,7 @@
}
units::volt_t kS{0};
- units::volt_t kCos{0};
+ units::volt_t kG{0};
units::unit_t<kv_unit> kV{0};
units::unit_t<ka_unit> kA{0};
};
diff --git a/wpimath/src/main/native/include/frc/controller/BangBangController.h b/wpimath/src/main/native/include/frc/controller/BangBangController.h
new file mode 100644
index 0000000..6058c81
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/BangBangController.h
@@ -0,0 +1,121 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <limits>
+
+#include <wpi/SymbolExports.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc {
+
+/**
+ * Implements a bang-bang controller, which outputs either 0 or 1 depending on
+ * whether the measurement is less than the setpoint. This maximally-aggressive
+ * control approach works very well for velocity control of high-inertia
+ * mechanisms, and poorly on most other things.
+ *
+ * <p>Note that this is an *asymmetric* bang-bang controller - it will not exert
+ * any control effort in the reverse direction (e.g. it won't try to slow down
+ * an over-speeding shooter wheel). This asymmetry is *extremely important.*
+ * Bang-bang control is extremely simple, but also potentially hazardous. Always
+ * ensure that your motor controllers are set to "coast" before attempting to
+ * control them with a bang-bang controller.
+ */
+class WPILIB_DLLEXPORT BangBangController
+ : public wpi::Sendable,
+ public wpi::SendableHelper<BangBangController> {
+ public:
+ /**
+ * Creates a new bang-bang controller.
+ *
+ * <p>Always ensure that your motor controllers are set to "coast" before
+ * attempting to control them with a bang-bang controller.
+ *
+ * @param tolerance Tolerance for atSetpoint.
+ */
+ explicit BangBangController(
+ double tolerance = std::numeric_limits<double>::infinity());
+
+ /**
+ * Sets the setpoint for the bang-bang controller.
+ *
+ * @param setpoint The desired setpoint.
+ */
+ void SetSetpoint(double setpoint);
+
+ /**
+ * Returns the current setpoint of the bang-bang controller.
+ *
+ * @return The current setpoint.
+ */
+ double GetSetpoint() const;
+
+ /**
+ * Returns true if the error is within the tolerance of the setpoint.
+ *
+ * @return Whether the error is within the acceptable bounds.
+ */
+ bool AtSetpoint() const;
+
+ /**
+ * Sets the error within which AtSetpoint will return true.
+ *
+ * @param tolerance Position error which is tolerable.
+ */
+ void SetTolerance(double tolerance);
+
+ /**
+ * Returns the current tolerance of the controller.
+ *
+ * @return The current tolerance.
+ */
+ double GetTolerance() const;
+
+ /**
+ * Returns the current measurement of the process variable.
+ *
+ * @return The current measurement of the process variable.
+ */
+ double GetMeasurement() const;
+
+ /**
+ * Returns the current error.
+ *
+ * @return The current error.
+ */
+ double GetError() const;
+
+ /**
+ * Returns the calculated control output.
+ *
+ * <p>Always ensure that your motor controllers are set to "coast" before
+ * attempting to control them with a bang-bang controller.
+ *
+ * @param measurement The most recent measurement of the process variable.
+ * @param setpoint The setpoint for the process variable.
+ * @return The calculated motor output (0 or 1).
+ */
+ double Calculate(double measurement, double setpoint);
+
+ /**
+ * Returns the calculated control output.
+ *
+ * @param measurement The most recent measurement of the process variable.
+ * @return The calculated motor output (0 or 1).
+ */
+ double Calculate(double measurement);
+
+ void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+ double m_tolerance;
+
+ double m_setpoint = 0;
+ double m_measurement = 0;
+};
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
index c934e0a..44a8501 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
@@ -135,7 +135,7 @@
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
- m_K = (B.transpose() * S * B + R)
+ m_K = (discB.transpose() * S * discB + R)
.llt()
.solve(discB.transpose() * S * discA + N.transpose());
diff --git a/wpimath/src/main/native/include/frc/controller/RamseteController.h b/wpimath/src/main/native/include/frc/controller/RamseteController.h
index 022fff9..bb23f46 100644
--- a/wpimath/src/main/native/include/frc/controller/RamseteController.h
+++ b/wpimath/src/main/native/include/frc/controller/RamseteController.h
@@ -5,11 +5,13 @@
#pragma once
#include <wpi/SymbolExports.h>
+#include <wpi/deprecated.h>
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/trajectory/Trajectory.h"
#include "units/angular_velocity.h"
+#include "units/length.h"
#include "units/velocity.h"
namespace frc {
@@ -43,22 +45,38 @@
*/
class WPILIB_DLLEXPORT RamseteController {
public:
+ using b_unit =
+ units::compound_unit<units::squared<units::radians>,
+ units::inverse<units::squared<units::meters>>>;
+ using zeta_unit = units::inverse<units::radians>;
+
/**
* Construct a Ramsete unicycle controller.
*
- * @param b Tuning parameter (b > 0) for which larger values make
+ * @param b Tuning parameter (b > 0 rad²/m²) for which larger values make
* convergence more aggressive like a proportional term.
- * @param zeta Tuning parameter (0 < zeta < 1) for which larger values provide
- * more damping in response.
+ * @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger
+ * values provide more damping in response.
*/
+ WPI_DEPRECATED("Use unit-safe constructor instead")
RamseteController(double b, double zeta);
/**
- * Construct a Ramsete unicycle controller. The default arguments for
- * b and zeta of 2.0 and 0.7 have been well-tested to produce desirable
- * results.
+ * Construct a Ramsete unicycle controller.
+ *
+ * @param b Tuning parameter (b > 0 rad²/m²) for which larger values make
+ * convergence more aggressive like a proportional term.
+ * @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger
+ * values provide more damping in response.
*/
- RamseteController() : RamseteController(2.0, 0.7) {}
+ RamseteController(units::unit_t<b_unit> b, units::unit_t<zeta_unit> zeta);
+
+ /**
+ * Construct a Ramsete unicycle controller. The default arguments for
+ * b and zeta of 2.0 rad²/m² and 0.7 rad⁻¹ have been well-tested to produce
+ * desirable results.
+ */
+ RamseteController();
/**
* Returns true if the pose error is within tolerance of the reference.
@@ -109,8 +127,8 @@
void SetEnabled(bool enabled);
private:
- double m_b;
- double m_zeta;
+ units::unit_t<b_unit> m_b;
+ units::unit_t<zeta_unit> m_zeta;
Pose2d m_poseError;
Pose2d m_poseTolerance;
diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
index b957c1e..12810fa 100644
--- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
@@ -38,8 +38,8 @@
* system containing x position, y position, heading, left encoder distance,
* and right encoder distance.
*
- * <strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity,
- * right wheel velocity, and change in gyro heading.
+ * <strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity,
+ * and angular velocity in the field coordinate system.
*
* NB: Using velocities make things considerably easier, because it means that
* teams don't have to worry about getting an accurate model. Basically, we
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
index aabb8ec..5cb8eb2 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
@@ -86,26 +86,46 @@
return;
}
- // We will perform a binary search to find the index of the element in the
- // vector that has a timestamp that is equal to or greater than the vision
- // measurement timestamp.
- auto lowerBoundIter = std::lower_bound(
+ // Perform a binary search to find the index of first snapshot whose
+ // timestamp is greater than or equal to the global measurement timestamp
+ auto it = std::lower_bound(
m_pastObserverSnapshots.cbegin(), m_pastObserverSnapshots.cend(),
timestamp,
[](const auto& entry, const auto& ts) { return entry.first < ts; });
- int index = std::distance(m_pastObserverSnapshots.cbegin(), lowerBoundIter);
- // High and Low should be the same. The sampled timestamp is greater than or
- // equal to the vision pose timestamp. We will now find the entry which is
- // closest in time to the requested timestamp.
+ size_t indexOfClosestEntry;
- size_t indexOfClosestEntry =
- units::math::abs(
- timestamp - m_pastObserverSnapshots[std::max(index - 1, 0)].first) <
- units::math::abs(timestamp -
- m_pastObserverSnapshots[index].first)
- ? index - 1
- : index;
+ if (it == m_pastObserverSnapshots.cbegin()) {
+ // If the global measurement is older than any snapshot, throw out the
+ // measurement because there's no state estimate into which to incorporate
+ // the measurement
+ if (timestamp < it->first) {
+ return;
+ }
+
+ // If the first snapshot has same timestamp as the global measurement, use
+ // that snapshot
+ indexOfClosestEntry = 0;
+ } else if (it == m_pastObserverSnapshots.cend()) {
+ // If all snapshots are older than the global measurement, use the newest
+ // snapshot
+ indexOfClosestEntry = m_pastObserverSnapshots.size() - 1;
+ } else {
+ // Index of snapshot taken after the global measurement
+ int nextIdx = std::distance(m_pastObserverSnapshots.cbegin(), it);
+
+ // Index of snapshot taken before the global measurement. Since we already
+ // handled the case where the index points to the first snapshot, this
+ // computation is guaranteed to be nonnegative.
+ int prevIdx = nextIdx - 1;
+
+ // Find the snapshot closest in time to global measurement
+ units::second_t prevTimeDiff =
+ units::math::abs(timestamp - m_pastObserverSnapshots[prevIdx].first);
+ units::second_t nextTimeDiff =
+ units::math::abs(timestamp - m_pastObserverSnapshots[nextIdx].first);
+ indexOfClosestEntry = prevTimeDiff < nextTimeDiff ? prevIdx : nextIdx;
+ }
units::second_t lastTimestamp =
m_pastObserverSnapshots[indexOfClosestEntry].first - nominalDt;
diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
index 93c9f1e..e9817ef 100644
--- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
@@ -38,8 +38,8 @@
* <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system
* containing x position, y position, and heading.
*
- * <strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity,
- * right wheel velocity, and change in gyro heading.
+ * <strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity,
+ * and angular velocity in the field coordinate system.
*
* <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
* position, and heading; or <strong> y = [theta]ᵀ </strong> containing gyro
diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
index 91e0e50..7422a3c 100644
--- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -40,8 +40,8 @@
* <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system
* containing x position, y position, and heading.
*
- * <strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity,
- * right wheel velocity, and change in gyro heading.
+ * <strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity,
+ * and angular velocity in the field coordinate system.
*
* <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
* position, and heading; or <strong> y = [theta]ᵀ </strong> containing gyro
diff --git a/wpimath/src/main/native/include/frc/filter/Debouncer.h b/wpimath/src/main/native/include/frc/filter/Debouncer.h
new file mode 100644
index 0000000..7307102
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/filter/Debouncer.h
@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/timestamp.h>
+
+#include "units/time.h"
+
+namespace frc {
+/**
+ * A simple debounce filter for boolean streams. Requires that the boolean
+ * change value from baseline for a specified period of time before the filtered
+ * value changes.
+ */
+class WPILIB_DLLEXPORT Debouncer {
+ public:
+ enum DebounceType { kRising, kFalling, kBoth };
+
+ /**
+ * Creates a new Debouncer.
+ *
+ * @param debounceTime The number of seconds the value must change from
+ * baseline for the filtered value to change.
+ * @param type Which type of state change the debouncing will be
+ * performed on.
+ */
+ explicit Debouncer(units::second_t debounceTime,
+ DebounceType type = DebounceType::kRising);
+
+ /**
+ * Applies the debouncer to the input stream.
+ *
+ * @param input The current value of the input stream.
+ * @return The debounced value of the input stream.
+ */
+ bool Calculate(bool input);
+
+ private:
+ units::second_t m_debounceTime;
+ bool m_baseline;
+ DebounceType m_debounceType;
+
+ units::second_t m_prevTime;
+
+ void ResetTimer();
+
+ bool HasElapsed() const;
+};
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
index 92d8bdc..6295327 100644
--- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h
+++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
@@ -10,6 +10,7 @@
#include <stdexcept>
#include <vector>
+#include <wpi/array.h>
#include <wpi/circular_buffer.h>
#include <wpi/span.h>
@@ -167,6 +168,73 @@
}
/**
+ * Creates a finite difference filter that computes the nth derivative of the
+ * input given the specified stencil points.
+ *
+ * Stencil points are the indices of the samples to use in the finite
+ * difference. 0 is the current sample, -1 is the previous sample, -2 is the
+ * sample before that, etc. Don't use positive stencil points (samples from
+ * the future) if the LinearFilter will be used for stream-based online
+ * filtering.
+ *
+ * @tparam Derivative The order of the derivative to compute.
+ * @tparam Samples The number of samples to use to compute the given
+ * derivative. This must be one more than the order of
+ * derivative or higher.
+ * @param stencil List of stencil points.
+ * @param period The period in seconds between samples taken by the user.
+ */
+ template <int Derivative, int Samples>
+ static LinearFilter<T> FiniteDifference(
+ const wpi::array<int, Samples> stencil, units::second_t period) {
+ // See
+ // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
+ //
+ // For a given list of stencil points s of length n and the order of
+ // derivative d < n, the finite difference coefficients can be obtained by
+ // solving the following linear system for the vector a.
+ //
+ // [s₁⁰ ⋯ sₙ⁰ ][a₁] [ δ₀,d ]
+ // [ ⋮ ⋱ ⋮ ][⋮ ] = d! [ ⋮ ]
+ // [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ] [δₙ₋₁,d]
+ //
+ // where δᵢ,ⱼ are the Kronecker delta. The FIR gains are the elements of the
+ // vector a in reverse order divided by hᵈ.
+ //
+ // The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
+
+ static_assert(Derivative >= 1,
+ "Order of derivative must be greater than or equal to one.");
+ static_assert(Samples > 0, "Number of samples must be greater than zero.");
+ static_assert(Derivative < Samples,
+ "Order of derivative must be less than number of samples.");
+
+ Eigen::Matrix<double, Samples, Samples> S;
+ for (int row = 0; row < Samples; ++row) {
+ for (int col = 0; col < Samples; ++col) {
+ S(row, col) = std::pow(stencil[col], row);
+ }
+ }
+
+ // Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
+ Eigen::Vector<double, Samples> d;
+ for (int i = 0; i < Samples; ++i) {
+ d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0;
+ }
+
+ Eigen::Vector<double, Samples> a =
+ S.householderQr().solve(d) / std::pow(period.value(), Derivative);
+
+ // Reverse gains list
+ std::vector<double> ffGains;
+ for (int i = Samples - 1; i >= 0; --i) {
+ ffGains.push_back(a(i));
+ }
+
+ return LinearFilter(ffGains, {});
+ }
+
+ /**
* Creates a backward finite difference filter that computes the nth
* derivative of the input given the specified number of samples.
*
@@ -184,56 +252,14 @@
* @param period The period in seconds between samples taken by the user.
*/
template <int Derivative, int Samples>
- static auto BackwardFiniteDifference(units::second_t period) {
- // See
- // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
- //
- // For a given list of stencil points s of length n and the order of
- // derivative d < n, the finite difference coefficients can be obtained by
- // solving the following linear system for the vector a.
- //
- // @verbatim
- // [s₁⁰ ⋯ sₙ⁰ ][a₁] [ δ₀,d ]
- // [ ⋮ ⋱ ⋮ ][⋮ ] = d! [ ⋮ ]
- // [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ] [δₙ₋₁,d]
- // @endverbatim
- //
- // where δᵢ,ⱼ are the Kronecker delta. For backward finite difference, the
- // stencil points are the range [-n + 1, 0]. The FIR gains are the elements
- // of the vector a in reverse order divided by hᵈ.
- //
- // The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
-
- static_assert(Derivative >= 1,
- "Order of derivative must be greater than or equal to one.");
- static_assert(Samples > 0, "Number of samples must be greater than zero.");
- static_assert(Derivative < Samples,
- "Order of derivative must be less than number of samples.");
-
- Eigen::Matrix<double, Samples, Samples> S;
- for (int row = 0; row < Samples; ++row) {
- for (int col = 0; col < Samples; ++col) {
- double s = 1 - Samples + col;
- S(row, col) = std::pow(s, row);
- }
- }
-
- // Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
- Eigen::Vector<double, Samples> d;
+ static LinearFilter<T> BackwardFiniteDifference(units::second_t period) {
+ // Generate stencil points from -(samples - 1) to 0
+ wpi::array<int, Samples> stencil{wpi::empty_array};
for (int i = 0; i < Samples; ++i) {
- d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0;
+ stencil[i] = -(Samples - 1) + i;
}
- Eigen::Vector<double, Samples> a =
- S.householderQr().solve(d) / std::pow(period.value(), Derivative);
-
- // Reverse gains list
- std::vector<double> gains;
- for (int i = Samples - 1; i >= 0; --i) {
- gains.push_back(a(i));
- }
-
- return LinearFilter(gains, {});
+ return FiniteDifference<Derivative, Samples>(stencil, period);
}
/**
diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h
index 9d7a856..6ad2b38 100644
--- a/wpimath/src/main/native/include/frc/geometry/Twist2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h
@@ -53,5 +53,15 @@
* @return Whether the two objects are not equal.
*/
bool operator!=(const Twist2d& other) const { return !operator==(other); }
+
+ /**
+ * Scale this by a given factor.
+ *
+ * @param factor The factor by which to scale.
+ * @return The scaled Twist2d.
+ */
+ Twist2d operator*(double factor) const {
+ return Twist2d{dx * factor, dy * factor, dtheta * factor};
+ }
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
new file mode 100644
index 0000000..a049e40
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
@@ -0,0 +1,128 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <array>
+#include <functional>
+#include <map>
+#include <utility>
+#include <vector>
+
+#include <wpi/MathExtras.h>
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * The TimeInterpolatableBuffer provides an easy way to estimate past
+ * measurements. One application might be in conjunction with the
+ * DifferentialDrivePoseEstimator, where knowledge of the robot pose at the time
+ * when vision or other global measurement were recorded is necessary, or for
+ * recording the past angles of mechanisms as measured by encoders.
+ *
+ * When sampling this buffer, a user-provided function or wpi::Lerp can be
+ * used. For Pose2ds, we use Twists.
+ *
+ * @tparam T The type stored in this buffer.
+ */
+template <typename T>
+class TimeInterpolatableBuffer {
+ public:
+ /**
+ * Create a new TimeInterpolatableBuffer.
+ *
+ * @param historySize The history size of the buffer.
+ * @param func The function used to interpolate between values.
+ */
+ TimeInterpolatableBuffer(units::second_t historySize,
+ std::function<T(const T&, const T&, double)> func)
+ : m_historySize(historySize), m_interpolatingFunc(func) {}
+
+ /**
+ * Create a new TimeInterpolatableBuffer. By default, the interpolation
+ * function is wpi::Lerp except for Pose2d, which uses the pose exponential.
+ *
+ * @param historySize The history size of the buffer.
+ */
+ explicit TimeInterpolatableBuffer(units::second_t historySize)
+ : m_historySize(historySize),
+ m_interpolatingFunc([](const T& start, const T& end, double t) {
+ return wpi::Lerp(start, end, t);
+ }) {}
+
+ /**
+ * Add a sample to the buffer.
+ *
+ * @param time The timestamp of the sample.
+ * @param sample The sample object.
+ */
+ void AddSample(units::second_t time, T sample) {
+ // Add the new state into the vector.
+ if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) {
+ m_pastSnapshots.emplace_back(time, sample);
+ } else {
+ m_pastSnapshots.insert(
+ std::upper_bound(
+ m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
+ [](auto t, const auto& pair) { return t < pair.first; }),
+ std::pair(time, sample));
+ }
+ while (time - m_pastSnapshots[0].first > m_historySize) {
+ m_pastSnapshots.erase(m_pastSnapshots.begin());
+ }
+ }
+
+ /** Clear all old samples. */
+ void Clear() { m_pastSnapshots.clear(); }
+
+ /**
+ * Sample the buffer at the given time. If there are no elements in the
+ * buffer, calling this function results in undefined behavior.
+ *
+ * @param time The time at which to sample the buffer.
+ */
+ T Sample(units::second_t time) {
+ // We will perform a binary search to find the index of the element in the
+ // vector that has a timestamp that is equal to or greater than the vision
+ // measurement timestamp.
+
+ if (time <= m_pastSnapshots.front().first) {
+ return m_pastSnapshots.front().second;
+ }
+ if (time > m_pastSnapshots.back().first) {
+ return m_pastSnapshots.back().second;
+ }
+ if (m_pastSnapshots.size() < 2) {
+ return m_pastSnapshots[0].second;
+ }
+
+ // Get the iterator which has a key no less than the requested key.
+ auto upper_bound = std::lower_bound(
+ m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
+ [](const auto& pair, auto t) { return t > pair.first; });
+
+ auto lower_bound = upper_bound - 1;
+
+ double t = ((time - lower_bound->first) /
+ (upper_bound->first - lower_bound->first));
+
+ return m_interpolatingFunc(lower_bound->second, upper_bound->second, t);
+ }
+
+ private:
+ units::second_t m_historySize;
+ std::vector<std::pair<units::second_t, T>> m_pastSnapshots;
+ std::function<T(const T&, const T&, double)> m_interpolatingFunc;
+};
+
+// Template specialization to ensure that Pose2d uses pose exponential
+template <>
+WPILIB_DLLEXPORT TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
+ units::second_t historySize);
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
index 70179de..8e992ef 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -6,7 +6,6 @@
#include <wpi/SymbolExports.h>
-#include "DifferentialDriveKinematics.h"
#include "frc/geometry/Pose2d.h"
#include "units/length.h"
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
index 2bf9fb9..fce2b96 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
@@ -24,15 +24,17 @@
units::meters_per_second_t right = 0_mps;
/**
- * Normalizes the wheel speeds using some max attainable speed. Sometimes,
- * after inverse kinematics, the requested speed from a/several modules may be
- * above the max attainable speed for the driving motor on that module. To fix
- * this issue, one can "normalize" all the wheel speeds to make sure that all
- * requested module speeds are below the absolute threshold, while maintaining
- * the ratio of speeds between modules.
+ * Renormalizes the wheel speeds if either side is above the specified
+ * maximum.
+ *
+ * Sometimes, after inverse kinematics, the requested speed from one or more
+ * wheels may be above the max attainable speed for the driving motor on that
+ * wheel. To fix this issue, one can reduce all the wheel speeds to make sure
+ * that all requested module speeds are at-or-below the absolute threshold,
+ * while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
- void Normalize(units::meters_per_second_t attainableMaxSpeed);
+ void Desaturate(units::meters_per_second_t attainableMaxSpeed);
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
index c24b134..e698c5f 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
@@ -34,15 +34,17 @@
units::meters_per_second_t rearRight = 0_mps;
/**
- * Normalizes the wheel speeds using some max attainable speed. Sometimes,
- * after inverse kinematics, the requested speed from a/several modules may be
- * above the max attainable speed for the driving motor on that module. To fix
- * this issue, one can "normalize" all the wheel speeds to make sure that all
- * requested module speeds are below the absolute threshold, while maintaining
- * the ratio of speeds between modules.
+ * Renormalizes the wheel speeds if any individual speed is above the
+ * specified maximum.
+ *
+ * Sometimes, after inverse kinematics, the requested speed from one or
+ * more wheels may be above the max attainable speed for the driving motor on
+ * that wheel. To fix this issue, one can reduce all the wheel speeds to make
+ * sure that all requested module speeds are at-or-below the absolute
+ * threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
- void Normalize(units::meters_per_second_t attainableMaxSpeed);
+ void Desaturate(units::meters_per_second_t attainableMaxSpeed);
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
index 84057ac..9801092 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -114,7 +114,7 @@
* @return An array containing the module states. Use caution because these
* module states are not normalized. Sometimes, a user input may cause one of
* the module speeds to go above the attainable max velocity. Use the
- * NormalizeWheelSpeeds(wpi::array<SwerveModuleState, NumModules>*,
+ * DesaturateWheelSpeeds(wpi::array<SwerveModuleState, NumModules>*,
* units::meters_per_second_t) function to rectify this issue. In addition,
* you can leverage the power of C++17 to directly assign the module states to
* variables:
@@ -159,18 +159,21 @@
wpi::array<SwerveModuleState, NumModules> moduleStates) const;
/**
- * Normalizes the wheel speeds using some max attainable speed. Sometimes,
- * after inverse kinematics, the requested speed from a/several modules may be
- * above the max attainable speed for the driving motor on that module. To fix
- * this issue, one can "normalize" all the wheel speeds to make sure that all
- * requested module speeds are below the absolute threshold, while maintaining
- * the ratio of speeds between modules.
+ * Renormalizes the wheel speeds if any individual speed is above the
+ * specified maximum.
+ *
+ * Sometimes, after inverse kinematics, the requested speed
+ * from one or more modules may be above the max attainable speed for the
+ * driving motor on that module. To fix this issue, one can reduce all the
+ * wheel speeds to make sure that all requested module speeds are at-or-below
+ * the absolute threshold, while maintaining the ratio of speeds between
+ * modules.
*
* @param moduleStates Reference to array of module states. The array will be
* mutated with the normalized speeds!
* @param attainableMaxSpeed The absolute max speed that a module can reach.
*/
- static void NormalizeWheelSpeeds(
+ static void DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed);
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index 1747453..7b958c1 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -88,7 +88,7 @@
}
template <size_t NumModules>
-void SwerveDriveKinematics<NumModules>::NormalizeWheelSpeeds(
+void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed) {
auto& states = *moduleStates;
diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
index 9ee2ea2..a18c03b 100644
--- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
+++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
@@ -54,7 +54,7 @@
: LinearSystemLoop(
plant, controller, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
- return frc::NormalizeInputVector<Inputs>(u, maxVoltage.value());
+ return frc::DesaturateInputVector<Inputs>(u, maxVoltage.value());
},
dt) {}
@@ -99,7 +99,7 @@
KalmanFilter<States, Inputs, Outputs>& observer, units::volt_t maxVoltage)
: LinearSystemLoop(controller, feedforward, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
- return frc::NormalizeInputVector<Inputs>(
+ return frc::DesaturateInputVector<Inputs>(
u, maxVoltage.value());
}) {}
diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
index c0f4506..26142bf 100644
--- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
+++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
@@ -283,7 +283,7 @@
// omega = 2/trackwidth v
//
// So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
- // to V/m/s).
+ // to V/(m/s).
return IdentifyDrivetrainSystem(kVlinear, kAlinear,
kVangular * 2.0 / trackwidth * 1_rad,
kAangular * 2.0 / trackwidth * 1_rad);
@@ -321,6 +321,38 @@
}
/**
+ * Constructs the state-space model for a DC motor motor.
+ *
+ * States: [[angular position, angular velocity]]
+ * Inputs: [[voltage]]
+ * Outputs: [[angular position, angular velocity]]
+ *
+ * @param motor Instance of DCMotor.
+ * @param J Moment of inertia.
+ * @param G Gear ratio from motor to carriage.
+ * @throws std::domain_error if J <= 0 or G <= 0.
+ */
+ static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor,
+ units::kilogram_square_meter_t J,
+ double G) {
+ if (J <= 0_kg_sq_m) {
+ throw std::domain_error("J must be greater than zero.");
+ }
+ if (G <= 0.0) {
+ throw std::domain_error("G must be greater than zero.");
+ }
+
+ Eigen::Matrix<double, 2, 2> A{
+ {0.0, 1.0},
+ {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
+ Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
+ Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+ Eigen::Matrix<double, 2, 1> D{0.0, 0.0};
+
+ return LinearSystem<2, 1, 2>(A, B, C, D);
+ }
+
+ /**
* Constructs the state-space model for a drivetrain.
*
* States: [[left velocity], [right velocity]]
diff --git a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
index 2fad345..d071ca3 100644
--- a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
+++ b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
@@ -157,20 +157,6 @@
private:
std::vector<State> m_states;
units::second_t m_totalTime = 0_s;
-
- /**
- * Linearly interpolates between two values.
- *
- * @param startValue The start value.
- * @param endValue The end value.
- * @param t The fraction for interpolation.
- *
- * @return The interpolated value.
- */
- template <typename T>
- static T Lerp(const T& startValue, const T& endValue, const double t) {
- return startValue + (endValue - startValue) * t;
- }
};
WPILIB_DLLEXPORT
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
index 84ec0e0..3df6c89 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
@@ -119,9 +119,7 @@
*
* @param func Error reporting function.
*/
- static void SetErrorHandler(std::function<void(const char*)> func) {
- s_errorFunc = std::move(func);
- }
+ static void SetErrorHandler(std::function<void(const char*)> func);
private:
static void ReportError(const char* error);
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
index 47a598e..4ec0f42 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
@@ -104,8 +104,6 @@
endAccel = units::math::max(endAccel, 0_s);
endFullSpeed = units::math::max(endFullSpeed, 0_s);
- units::second_t endDeccel = m_endDeccel - endAccel - endFullSpeed;
- endDeccel = units::math::max(endDeccel, 0_s);
const Acceleration_t acceleration = m_constraints.maxAcceleration;
const Acceleration_t decceleration = -m_constraints.maxAcceleration;
@@ -127,20 +125,16 @@
deccelVelocity = velocity;
}
- Distance_t deccelDist =
- deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
-
- deccelDist = units::math::max(deccelDist, Distance_t(0));
-
Distance_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+ Distance_t deccelDist;
if (accelDist > distToTarget) {
accelDist = distToTarget;
- fullSpeedDist = Distance_t(0);
- deccelDist = Distance_t(0);
+ fullSpeedDist = Distance_t{0};
+ deccelDist = Distance_t{0};
} else if (accelDist + fullSpeedDist > distToTarget) {
fullSpeedDist = distToTarget - accelDist;
- deccelDist = Distance_t(0);
+ deccelDist = Distance_t{0};
} else {
deccelDist = distToTarget - fullSpeedDist - accelDist;
}
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
index ad643bf..0edd8cc 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
@@ -32,7 +32,7 @@
units::meters_per_second_t speed) const override;
private:
- const DifferentialDriveKinematics& m_kinematics;
+ DifferentialDriveKinematics m_kinematics;
units::meters_per_second_t m_maxSpeed;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
index 06d0e50..40a0d8e 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
@@ -44,8 +44,8 @@
units::meters_per_second_t speed) const override;
private:
- const SimpleMotorFeedforward<units::meter>& m_feedforward;
- const DifferentialDriveKinematics& m_kinematics;
+ SimpleMotorFeedforward<units::meter> m_feedforward;
+ DifferentialDriveKinematics m_kinematics;
units::volt_t m_maxVoltage;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
index 816d8ef..ac47dbf 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
@@ -33,7 +33,7 @@
units::meters_per_second_t speed) const override;
private:
- const MecanumDriveKinematics& m_kinematics;
+ MecanumDriveKinematics m_kinematics;
units::meters_per_second_t m_maxSpeed;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
index 67e9fc9..d264977 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
@@ -32,7 +32,7 @@
units::meters_per_second_t speed) const override;
private:
- const frc::SwerveDriveKinematics<NumModules>& m_kinematics;
+ frc::SwerveDriveKinematics<NumModules> m_kinematics;
units::meters_per_second_t m_maxSpeed;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
index 1a1e4b8..5c4e5ac 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
@@ -24,7 +24,7 @@
auto yVelocity = velocity * pose.Rotation().Sin();
auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
{xVelocity, yVelocity, velocity * curvature});
- m_kinematics.NormalizeWheelSpeeds(&wheelSpeeds, m_maxSpeed);
+ m_kinematics.DesaturateWheelSpeeds(&wheelSpeeds, m_maxSpeed);
auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
diff --git a/wpimath/src/main/native/include/units/angular_velocity.h b/wpimath/src/main/native/include/units/angular_velocity.h
index 16f39e1..c9cc567 100644
--- a/wpimath/src/main/native/include/units/angular_velocity.h
+++ b/wpimath/src/main/native/include/units/angular_velocity.h
@@ -46,6 +46,8 @@
unit<std::ratio<1>, units::category::angular_velocity_unit>)
UNIT_ADD(angular_velocity, degrees_per_second, degrees_per_second, deg_per_s,
compound_unit<angle::degrees, inverse<time::seconds>>)
+UNIT_ADD(angular_velocity, turns_per_second, turns_per_second, tps,
+ compound_unit<angle::turns, inverse<time::seconds>>)
UNIT_ADD(angular_velocity, revolutions_per_minute, revolutions_per_minute, rpm,
unit<std::ratio<2, 60>, radians_per_second, std::ratio<1>>)
UNIT_ADD(angular_velocity, milliarcseconds_per_year, milliarcseconds_per_year,
diff --git a/wpimath/src/main/native/include/units/base.h b/wpimath/src/main/native/include/units/base.h
index f2d45cf..bc9fb44 100644
--- a/wpimath/src/main/native/include/units/base.h
+++ b/wpimath/src/main/native/include/units/base.h
@@ -366,6 +366,7 @@
namespace traits\
{\
template<typename... T> struct is_ ## unitCategory ## _unit : std::integral_constant<bool, units::all_true<units::traits::detail::is_ ## unitCategory ## _unit_impl<std::decay_t<T>>::value...>::value> {};\
+ template<typename... T> inline constexpr bool is_ ## unitCategory ## _unit_v = is_ ## unitCategory ## _unit<T...>::value;\
}
#else
#define UNIT_ADD_IS_UNIT_CATEGORY_TRAIT(unitCategory)\
@@ -375,6 +376,8 @@
struct is_ ## unitCategory ## _unit : std::integral_constant<bool, units::traits::detail::is_ ## unitCategory ## _unit_impl<typename std::decay<T1>::type>::value &&\
units::traits::detail::is_ ## unitCategory ## _unit_impl<typename std::decay<T2>::type>::value &&\
units::traits::detail::is_ ## unitCategory ## _unit_impl<typename std::decay<T3>::type>::value>{};\
+ template<typename T1, typename T2 = T1, typename T3 = T1>\
+ inline constexpr bool is_ ## unitCategory ## _unit_v = is_ ## unitCategory ## _unit<T1, T2, T3>::value;\
}
#endif
@@ -589,6 +592,8 @@
has_num<T>::value &&
has_den<T>::value>
{};
+ template<class T>
+ inline constexpr bool is_ratio_v = is_ratio<T>::value;
}
//------------------------------
@@ -613,6 +618,8 @@
*/
template<bool... Args>
struct all_true : std::is_same<units::bool_pack<true, Args...>, units::bool_pack<Args..., true>> {};
+ template<bool... Args>
+ inline constexpr bool all_true_t_v = all_true<Args...>::type::value;
/** @endcond */ // DOXYGEN IGNORE
/**
@@ -713,6 +720,8 @@
*/
template<class T>
struct is_unit : std::is_base_of<units::detail::_unit, T>::type {};
+ template<class T>
+ inline constexpr bool is_unit_v = is_unit<T>::value;
}
/** @} */ // end of TypeTraits
@@ -1521,6 +1530,8 @@
template<class U1, class U2>
struct is_convertible_unit : std::is_same <traits::base_unit_of<typename units::traits::unit_traits<U1>::base_unit_type>,
base_unit_of<typename units::traits::unit_traits<U2>::base_unit_type >> {};
+ template<class U1, class U2>
+ inline constexpr bool is_convertible_unit_v = is_convertible_unit<U1, U2>::value;
}
//------------------------------
@@ -1714,6 +1725,8 @@
*/
template<class T, class Ret>
struct has_value_member : traits::detail::has_value_member_impl<T, Ret>::type {};
+ template<class T, class Ret>
+ inline constexpr bool has_value_member_v = has_value_member<T, Ret>::value;
}
/** @endcond */ // END DOXYGEN IGNORE
@@ -1852,6 +1865,8 @@
*/
template<class T>
struct is_unit_t : std::is_base_of<units::detail::_unit_t, T>::type {};
+ template<class T>
+ inline constexpr bool is_unit_t_v = is_unit_t<T>::value;
}
/**
@@ -2401,12 +2416,16 @@
#if !defined(_MSC_VER) || _MSC_VER > 1800 // bug in VS2013 prevents this from working
template<typename... T>
struct has_linear_scale : std::integral_constant<bool, units::all_true<std::is_base_of<units::linear_scale<typename units::traits::unit_t_traits<T>::underlying_type>, T>::value...>::value > {};
+ template<typename... T>
+ inline constexpr bool has_linear_scale_v = has_linear_scale<T...>::value;
#else
template<typename T1, typename T2 = T1, typename T3 = T1>
struct has_linear_scale : std::integral_constant<bool,
std::is_base_of<units::linear_scale<typename units::traits::unit_t_traits<T1>::underlying_type>, T1>::value &&
std::is_base_of<units::linear_scale<typename units::traits::unit_t_traits<T2>::underlying_type>, T2>::value &&
std::is_base_of<units::linear_scale<typename units::traits::unit_t_traits<T3>::underlying_type>, T3>::value> {};
+ template<typename T1, typename T2 = T1, typename T3 = T1>
+ inline constexpr bool has_linear_scale_v = has_linear_scale<T1, T2, T3>::value;
#endif
/**
@@ -2419,12 +2438,16 @@
#if !defined(_MSC_VER) || _MSC_VER > 1800 // bug in VS2013 prevents this from working
template<typename... T>
struct has_decibel_scale : std::integral_constant<bool, units::all_true<std::is_base_of<units::decibel_scale<typename units::traits::unit_t_traits<T>::underlying_type>, T>::value...>::value> {};
+ template<typename... T>
+ inline constexpr bool has_decibel_scale_v = has_decibel_scale<T...>::value;
#else
template<typename T1, typename T2 = T1, typename T3 = T1>
struct has_decibel_scale : std::integral_constant<bool,
std::is_base_of<units::decibel_scale<typename units::traits::unit_t_traits<T1>::underlying_type>, T1>::value &&
std::is_base_of<units::decibel_scale<typename units::traits::unit_t_traits<T2>::underlying_type>, T2>::value &&
std::is_base_of<units::decibel_scale<typename units::traits::unit_t_traits<T2>::underlying_type>, T3>::value> {};
+ template<typename T1, typename T2 = T1, typename T3 = T1>
+ inline constexpr bool has_decibel_scale_v = has_decibel_scale<T1, T2, T3>::value;
#endif
/**
@@ -2439,6 +2462,8 @@
struct is_same_scale : std::integral_constant<bool,
std::is_same<typename units::traits::unit_t_traits<T1>::non_linear_scale_type, typename units::traits::unit_t_traits<T2>::non_linear_scale_type>::value>
{};
+ template<typename T1, typename T2>
+ inline constexpr bool is_same_scale_v = is_same_scale<T1, T2>::value;
}
//----------------------------------
@@ -3033,6 +3058,8 @@
struct is_unit_value_t : std::integral_constant<bool,
std::is_base_of<units::detail::_unit_value_t<Units>, T>::value>
{};
+ template<typename T, typename Units = typename traits::unit_value_t_traits<T>::unit_type>
+ inline constexpr bool is_unit_value_t_v = is_unit_value_t<T, Units>::value;
/**
* @ingroup TypeTraits
@@ -3045,6 +3072,8 @@
{
static_assert(is_base_unit<Category>::value, "Template parameter `Category` must be a `base_unit` type.");
};
+ template<typename Category, typename T>
+ inline constexpr bool is_unit_value_t_category_v = is_unit_value_t_category<Category, T>::value;
}
/** @cond */ // DOXYGEN IGNORE
diff --git a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java b/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
index 4140fae..66daf3d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
@@ -11,7 +11,7 @@
import org.junit.jupiter.api.Test;
@SuppressWarnings({"ParameterName", "LocalVariableName"})
-public class DrakeTest {
+class DrakeTest {
public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
for (int i = 0; i < A.numRows(); i++) {
for (int j = 0; j < A.numCols(); j++) {
@@ -49,7 +49,7 @@
}
@Test
- public void testDiscreteAlgebraicRicattiEquation() {
+ void testDiscreteAlgebraicRicattiEquation() {
int n1 = 4;
int m1 = 1;
diff --git a/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java b/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java
index 45df8fe..8ee11e5 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java
@@ -15,7 +15,7 @@
import org.ejml.data.SingularMatrixException;
import org.junit.jupiter.api.Test;
-public class MatrixTest {
+class MatrixTest {
@Test
void testMatrixMultiplication() {
var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 0.0, 1.0);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
index 456506c..8cd1e5a 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
@@ -18,9 +18,9 @@
import org.ejml.simple.SimpleMatrix;
import org.junit.jupiter.api.Test;
-public class StateSpaceUtilTest {
+class StateSpaceUtilTest {
@Test
- public void testCostArray() {
+ void testCostArray() {
var mat = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(1.0, 2.0, 3.0));
assertEquals(1.0, mat.get(0, 0), 1e-3);
@@ -35,7 +35,7 @@
}
@Test
- public void testCovArray() {
+ void testCovArray() {
var mat = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), VecBuilder.fill(1.0, 2.0, 3.0));
assertEquals(1.0, mat.get(0, 0), 1e-3);
@@ -51,7 +51,7 @@
@Test
@SuppressWarnings("LocalVariableName")
- public void testIsStabilizable() {
+ void testIsStabilizable() {
Matrix<N2, N2> A;
Matrix<N2, N1> B = VecBuilder.fill(0, 1);
@@ -78,7 +78,7 @@
@Test
@SuppressWarnings("LocalVariableName")
- public void testIsDetectable() {
+ void testIsDetectable() {
Matrix<N2, N2> A;
Matrix<N1, N2> C = Matrix.mat(Nat.N1(), Nat.N2()).fill(0, 1);
@@ -104,7 +104,7 @@
}
@Test
- public void testMakeWhiteNoiseVector() {
+ void testMakeWhiteNoiseVector() {
var firstData = new ArrayList<Double>();
var secondData = new ArrayList<Double>();
for (int i = 0; i < 1000; i++) {
@@ -135,7 +135,7 @@
}
@Test
- public void testMatrixExp() {
+ void testMatrixExp() {
Matrix<N2, N2> wrappedMatrix = Matrix.eye(Nat.N2());
var wrappedResult = wrappedMatrix.exp();
@@ -152,7 +152,7 @@
}
@Test
- public void testSimpleMatrixExp() {
+ void testSimpleMatrixExp() {
SimpleMatrix matrix = SimpleMatrixUtils.eye(2);
var result = SimpleMatrixUtils.exp(matrix);
@@ -175,7 +175,7 @@
}
@Test
- public void testPoseToVector() {
+ void testPoseToVector() {
Pose2d pose = new Pose2d(1, 2, new Rotation2d(3));
var vector = StateSpaceUtil.poseToVector(pose);
assertEquals(pose.getTranslation().getX(), vector.get(0, 0), 1e-6);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/BangBangInputOutputTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/BangBangInputOutputTest.java
new file mode 100644
index 0000000..9e1f6f2
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/BangBangInputOutputTest.java
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class BangBangInputOutputTest {
+ private BangBangController m_controller;
+
+ @BeforeEach
+ void setUp() {
+ m_controller = new BangBangController();
+ }
+
+ @Test
+ void shouldOutput() {
+ assertEquals(m_controller.calculate(0, 1), 1);
+ }
+
+ @Test
+ void shouldNotOutput() {
+ assertEquals(m_controller.calculate(1, 0), 0);
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/BangBangToleranceTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/BangBangToleranceTest.java
new file mode 100644
index 0000000..bcf688b
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/BangBangToleranceTest.java
@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class BangBangToleranceTest {
+ private BangBangController m_controller;
+
+ @BeforeEach
+ void setUp() {
+ m_controller = new BangBangController(0.1);
+ }
+
+ @Test
+ void inTolerance() {
+ m_controller.setSetpoint(1);
+ m_controller.calculate(1);
+ assertTrue(m_controller.atSetpoint());
+ }
+
+ @Test
+ void outOfTolerance() {
+ m_controller.setSetpoint(1);
+ m_controller.calculate(0);
+ assertFalse(m_controller.atSetpoint());
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
index 5d9c2b8..93b89c8 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
@@ -6,15 +6,19 @@
import static org.junit.jupiter.api.Assertions.assertEquals;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import org.junit.jupiter.api.Test;
-public class LinearQuadraticRegulatorTest {
+class LinearQuadraticRegulatorTest {
@Test
@SuppressWarnings("LocalVariableName")
- public void testLQROnElevator() {
+ void testLQROnElevator() {
var motors = DCMotor.getVex775Pro(2);
var m = 5.0;
@@ -27,32 +31,32 @@
var rElms = VecBuilder.fill(12.0);
var dt = 0.00505;
- var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt);
+ var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK();
- var k = controller.getK();
-
- assertEquals(522.153, k.get(0, 0), 0.1);
- assertEquals(38.2, k.get(0, 1), 0.1);
+ assertEquals(522.153, K.get(0, 0), 0.1);
+ assertEquals(38.2, K.get(0, 1), 0.1);
}
@Test
- public void testFourMotorElevator() {
+ @SuppressWarnings("LocalVariableName")
+ void testFourMotorElevator() {
var dt = 0.020;
var plant =
LinearSystemId.createElevatorSystem(
DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67);
- var regulator =
- new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt);
+ var K =
+ new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt)
+ .getK();
- assertEquals(10.381, regulator.getK().get(0, 0), 1e-2);
- assertEquals(0.6929, regulator.getK().get(0, 1), 1e-2);
+ assertEquals(10.381, K.get(0, 0), 1e-2);
+ assertEquals(0.6929, K.get(0, 1), 1e-2);
}
@Test
@SuppressWarnings("LocalVariableName")
- public void testLQROnArm() {
+ void testLQROnArm() {
var motors = DCMotor.getVex775Pro(2);
var m = 4.0;
@@ -65,16 +69,99 @@
var rElms = VecBuilder.fill(12.0);
var dt = 0.00505;
- var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt);
+ var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK();
- var k = controller.getK();
+ assertEquals(19.16, K.get(0, 0), 0.1);
+ assertEquals(3.32, K.get(0, 1), 0.1);
+ }
- assertEquals(19.16, k.get(0, 0), 0.1);
- assertEquals(3.32, k.get(0, 1), 0.1);
+ /**
+ * Returns feedback control gain for implicit model following.
+ *
+ * <p>This is used to test the QRN overload of LQR.
+ *
+ * @param States Number of states.
+ * @param Inputs Number of inputs.
+ * @param A State matrix.
+ * @param B Input matrix.
+ * @param Q State cost matrix.
+ * @param R Input cost matrix.
+ * @param Aref Desired state matrix.
+ * @param dtSeconds Discretization timestep in seconds.
+ */
+ @SuppressWarnings({"LocalVariableName", "MethodTypeParameterName", "ParameterName"})
+ <States extends Num, Inputs extends Num> Matrix<Inputs, States> getImplicitModelFollowingK(
+ Matrix<States, States> A,
+ Matrix<States, Inputs> B,
+ Matrix<States, States> Q,
+ Matrix<Inputs, Inputs> R,
+ Matrix<States, States> Aref,
+ double dtSeconds) {
+ // Discretize real dynamics
+ var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
+ var discA = discABPair.getFirst();
+ var discB = discABPair.getSecond();
+
+ // Discretize desired dynamics
+ var discAref = Discretization.discretizeA(Aref, dtSeconds);
+
+ Matrix<States, States> Qimf =
+ (discA.minus(discAref)).transpose().times(Q).times(discA.minus(discAref));
+ Matrix<Inputs, Inputs> Rimf = discB.transpose().times(Q).times(discB).plus(R);
+ Matrix<States, Inputs> Nimf = (discA.minus(discAref)).transpose().times(Q).times(discB);
+
+ return new LinearQuadraticRegulator<>(A, B, Qimf, Rimf, Nimf, dtSeconds).getK();
}
@Test
- public void testLatencyCompensate() {
+ @SuppressWarnings("LocalVariableName")
+ void testMatrixOverloadsWithSingleIntegrator() {
+ var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0);
+ var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+ var Q = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+ var R = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+
+ // QR overload
+ var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK();
+ assertEquals(0.99750312499512261, K.get(0, 0), 1e-10);
+ assertEquals(0.0, K.get(0, 1), 1e-10);
+ assertEquals(0.0, K.get(1, 0), 1e-10);
+ assertEquals(0.99750312499512261, K.get(1, 1), 1e-10);
+
+ // QRN overload
+ var N = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+ var Kimf = new LinearQuadraticRegulator<>(A, B, Q, R, N, 0.005).getK();
+ assertEquals(1.0, Kimf.get(0, 0), 1e-10);
+ assertEquals(0.0, Kimf.get(0, 1), 1e-10);
+ assertEquals(0.0, Kimf.get(1, 0), 1e-10);
+ assertEquals(1.0, Kimf.get(1, 1), 1e-10);
+ }
+
+ @Test
+ @SuppressWarnings("LocalVariableName")
+ void testMatrixOverloadsWithDoubleIntegrator() {
+ double Kv = 3.02;
+ double Ka = 0.642;
+
+ var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / Ka);
+ var B = Matrix.mat(Nat.N2(), Nat.N1()).fill(0, 1.0 / Ka);
+ var Q = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.2);
+ var R = Matrix.mat(Nat.N1(), Nat.N1()).fill(0.25);
+
+ // QR overload
+ var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK();
+ assertEquals(1.9960017786537287, K.get(0, 0), 1e-10);
+ assertEquals(0.51182128351092726, K.get(0, 1), 1e-10);
+
+ // QRN overload
+ var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 2.0));
+ var Kimf = getImplicitModelFollowingK(A, B, Q, R, Aref, 0.005);
+ assertEquals(0.0, Kimf.get(0, 0), 1e-10);
+ assertEquals(-5.367540084534802e-05, Kimf.get(0, 1), 1e-10);
+ }
+
+ @Test
+ void testLatencyCompensate() {
var dt = 0.02;
var plant =
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
index 597ae7f..5fa4939 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
@@ -21,8 +21,8 @@
import java.util.Random;
import org.junit.jupiter.api.Test;
-public class LinearSystemLoopTest {
- public static final double kDt = 0.00505;
+class LinearSystemLoopTest {
+ private static final double kDt = 0.00505;
private static final double kPositionStddev = 0.0001;
private static final Random random = new Random();
@@ -51,7 +51,7 @@
@Test
@SuppressWarnings("LocalVariableName")
- public void testStateSpaceEnabled() {
+ void testStateSpaceEnabled() {
m_loop.reset(VecBuilder.fill(0, 0));
Matrix<N2, N1> references = VecBuilder.fill(2.0, 0.0);
var constraints = new TrapezoidProfile.Constraints(4, 3);
@@ -80,7 +80,7 @@
@Test
@SuppressWarnings("LocalVariableName")
- public void testFlywheelEnabled() {
+ void testFlywheelEnabled() {
LinearSystem<N1, N1, N1> plant =
LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00289, 1.0);
KalmanFilter<N1, N1, N1> observer =
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java
index 9fcf5e3..c36e72f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java
@@ -11,9 +11,9 @@
import edu.wpi.first.math.VecBuilder;
import org.junit.jupiter.api.Test;
-public class AngleStatisticsTest {
+class AngleStatisticsTest {
@Test
- public void testMean() {
+ void testMean() {
// 3 states, 2 sigmas
var sigmas =
Matrix.mat(Nat.N3(), Nat.N2()).fill(1, 1.2, Math.toRadians(359), Math.toRadians(3), 1, 2);
@@ -27,7 +27,7 @@
}
@Test
- public void testResidual() {
+ void testResidual() {
var first = VecBuilder.fill(1, Math.toRadians(1), 2);
var second = VecBuilder.fill(1, Math.toRadians(359), 1);
assertTrue(
@@ -36,7 +36,7 @@
}
@Test
- public void testAdd() {
+ void testAdd() {
var first = VecBuilder.fill(1, Math.toRadians(1), 2);
var second = VecBuilder.fill(1, Math.toRadians(359), 1);
assertTrue(AngleStatistics.angleAdd(first, second, 1).isEqual(VecBuilder.fill(2, 0, 3), 1e-6));
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
index 6e4b261..78908e4 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
@@ -21,10 +21,10 @@
import java.util.Random;
import org.junit.jupiter.api.Test;
-public class DifferentialDrivePoseEstimatorTest {
+class DifferentialDrivePoseEstimatorTest {
@SuppressWarnings("LocalVariableName")
@Test
- public void testAccuracy() {
+ void testAccuracy() {
var estimator =
new DifferentialDrivePoseEstimator(
new Rotation2d(),
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
index 18b095b..5b07ebc 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
@@ -27,8 +27,8 @@
import org.junit.jupiter.api.Test;
@SuppressWarnings("ParameterName")
-public class ExtendedKalmanFilterTest {
- public static Matrix<N5, N1> getDynamics(final Matrix<N5, N1> x, final Matrix<N2, N1> u) {
+class ExtendedKalmanFilterTest {
+ private static Matrix<N5, N1> getDynamics(final Matrix<N5, N1> x, final Matrix<N2, N1> u) {
final var motors = DCMotor.getCIM(2);
final var gr = 7.08; // Gear ratio
@@ -58,17 +58,19 @@
return result;
}
- public static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+ @SuppressWarnings("PMD.UnusedFormalParameter")
+ private static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
}
- public static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+ @SuppressWarnings("PMD.UnusedFormalParameter")
+ private static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
return VecBuilder.fill(x.get(0, 0), x.get(1, 0), x.get(2, 0), x.get(3, 0), x.get(4, 0));
}
@SuppressWarnings("LocalVariableName")
@Test
- public void testInit() {
+ void testInit() {
double dtSeconds = 0.00505;
assertDoesNotThrow(
@@ -99,7 +101,7 @@
@SuppressWarnings("LocalVariableName")
@Test
- public void testConvergence() {
+ void testConvergence() {
double dtSeconds = 0.00505;
double rbMeters = 0.8382 / 2.0; // Robot radius
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
index 0a18497..4d709be 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
@@ -25,7 +25,7 @@
import java.util.Random;
import org.junit.jupiter.api.Test;
-public class KalmanFilterTest {
+class KalmanFilterTest {
private static LinearSystem<N2, N1, N1> elevatorPlant;
private static final double kDt = 0.00505;
@@ -35,7 +35,7 @@
}
@SuppressWarnings("LocalVariableName")
- public static void createElevator() {
+ private static void createElevator() {
var motors = DCMotor.getVex775Pro(2);
var m = 5.0;
@@ -62,7 +62,7 @@
@Test
@SuppressWarnings("LocalVariableName")
- public void testElevatorKalmanFilter() {
+ void testElevatorKalmanFilter() {
var Q = VecBuilder.fill(0.05, 1.0);
var R = VecBuilder.fill(0.0001);
@@ -70,7 +70,7 @@
}
@Test
- public void testSwerveKFStationary() {
+ void testSwerveKFStationary() {
var random = new Random();
var filter =
@@ -99,7 +99,7 @@
}
@Test
- public void testSwerveKFMovingWithoutAccelerating() {
+ void testSwerveKFMovingWithoutAccelerating() {
var random = new Random();
var filter =
@@ -137,7 +137,7 @@
@Test
@SuppressWarnings("LocalVariableName")
- public void testSwerveKFMovingOverTrajectory() {
+ void testSwerveKFMovingOverTrajectory() {
var random = new Random();
var filter =
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
index 38b0d20..d8756f0 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
@@ -18,10 +18,10 @@
import java.util.Random;
import org.junit.jupiter.api.Test;
-public class MecanumDrivePoseEstimatorTest {
+class MecanumDrivePoseEstimatorTest {
@Test
@SuppressWarnings("LocalVariableName")
- public void testAccuracy() {
+ void testAccuracy() {
var kinematics =
new MecanumDriveKinematics(
new Translation2d(1, 1), new Translation2d(1, -1),
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
index b6e32fc..3c51d6c 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
@@ -11,9 +11,9 @@
import edu.wpi.first.math.VecBuilder;
import org.junit.jupiter.api.Test;
-public class MerweScaledSigmaPointsTest {
+class MerweScaledSigmaPointsTest {
@Test
- public void testZeroMeanPoints() {
+ void testZeroMeanPoints() {
var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
var points =
merweScaledSigmaPoints.sigmaPoints(
@@ -28,7 +28,7 @@
}
@Test
- public void testNonzeroMeanPoints() {
+ void testNonzeroMeanPoints() {
var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
var points =
merweScaledSigmaPoints.sigmaPoints(
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
index 607e8de..c6126c6 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
@@ -18,10 +18,10 @@
import java.util.Random;
import org.junit.jupiter.api.Test;
-public class SwerveDrivePoseEstimatorTest {
+class SwerveDrivePoseEstimatorTest {
@Test
@SuppressWarnings("LocalVariableName")
- public void testAccuracy() {
+ void testAccuracy() {
var kinematics =
new SwerveDriveKinematics(
new Translation2d(1, 1),
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
index 1264591..25def49 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
@@ -29,9 +29,9 @@
import java.util.List;
import org.junit.jupiter.api.Test;
-public class UnscentedKalmanFilterTest {
+class UnscentedKalmanFilterTest {
@SuppressWarnings({"LocalVariableName", "ParameterName"})
- public static Matrix<N6, N1> getDynamics(Matrix<N6, N1> x, Matrix<N2, N1> u) {
+ private static Matrix<N6, N1> getDynamics(Matrix<N6, N1> x, Matrix<N2, N1> u) {
var motors = DCMotor.getCIM(2);
var gHigh = 7.08;
@@ -69,19 +69,19 @@
k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)));
}
- @SuppressWarnings("ParameterName")
- public static Matrix<N4, N1> getLocalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
+ @SuppressWarnings({"PMD.UnusedFormalParameter", "ParameterName"})
+ private static Matrix<N4, N1> getLocalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0), x.get(5, 0));
}
- @SuppressWarnings("ParameterName")
- public static Matrix<N6, N1> getGlobalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
+ @SuppressWarnings({"PMD.UnusedFormalParameter", "ParameterName"})
+ private static Matrix<N6, N1> getGlobalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
return x.copy();
}
@Test
@SuppressWarnings("LocalVariableName")
- public void testInit() {
+ void testInit() {
assertDoesNotThrow(
() -> {
UnscentedKalmanFilter<N6, N2, N4> observer =
@@ -104,7 +104,7 @@
@SuppressWarnings("LocalVariableName")
@Test
- public void testConvergence() {
+ void testConvergence() {
double dtSeconds = 0.00505;
double rbMeters = 0.8382 / 2.0; // Robot radius
@@ -206,7 +206,7 @@
@Test
@SuppressWarnings({"LocalVariableName", "ParameterName"})
- public void testLinearUKF() {
+ void testLinearUKF() {
var dt = 0.020;
var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
var observer =
@@ -236,7 +236,7 @@
}
@Test
- public void testUnscentedTransform() {
+ void testUnscentedTransform() {
// From FilterPy
var ret =
UnscentedKalmanFilter.unscentedTransform(
diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java
new file mode 100644
index 0000000..b6334de
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java
@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.filter;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.util.WPIUtilJNI;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class DebouncerTest {
+ @BeforeEach
+ void setUp() {
+ WPIUtilJNI.enableMockTime();
+ WPIUtilJNI.setMockTime(0L);
+ }
+
+ @AfterEach
+ void tearDown() {
+ WPIUtilJNI.setMockTime(0L);
+ WPIUtilJNI.disableMockTime();
+ }
+
+ @Test
+ void debounceRisingTest() {
+ var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kRising);
+
+ debouncer.calculate(false);
+ assertFalse(debouncer.calculate(true));
+
+ WPIUtilJNI.setMockTime(1000000L);
+
+ assertTrue(debouncer.calculate(true));
+ }
+
+ @Test
+ void debounceFallingTest() {
+ var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kFalling);
+
+ debouncer.calculate(true);
+ assertTrue(debouncer.calculate(false));
+
+ WPIUtilJNI.setMockTime(1000000L);
+
+ assertFalse(debouncer.calculate(false));
+
+ WPIUtilJNI.setMockTime(0L);
+ }
+
+ @Test
+ void debounceBothTest() {
+ var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kBoth);
+
+ debouncer.calculate(false);
+ assertFalse(debouncer.calculate(true));
+
+ WPIUtilJNI.setMockTime(1000000L);
+
+ assertTrue(debouncer.calculate(true));
+ assertTrue(debouncer.calculate(false));
+
+ WPIUtilJNI.setMockTime(2000000L);
+
+ assertFalse(debouncer.calculate(false));
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
index 805129f..f0b39c6 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
@@ -109,12 +109,84 @@
0.0));
}
+ /** Test central finite difference. */
+ @Test
+ void centralFiniteDifferenceTest() {
+ double h = 0.005;
+
+ assertCentralResults(
+ 1,
+ 3,
+ // f(x) = x²
+ (double x) -> x * x,
+ // df/dx = 2x
+ (double x) -> 2.0 * x,
+ h,
+ -20.0,
+ 20.0);
+
+ assertCentralResults(
+ 1,
+ 3,
+ // f(x) = sin(x)
+ (double x) -> Math.sin(x),
+ // df/dx = cos(x)
+ (double x) -> Math.cos(x),
+ h,
+ -20.0,
+ 20.0);
+
+ assertCentralResults(
+ 1,
+ 3,
+ // f(x) = ln(x)
+ (double x) -> Math.log(x),
+ // df/dx = 1 / x
+ (double x) -> 1.0 / x,
+ h,
+ 1.0,
+ 20.0);
+
+ assertCentralResults(
+ 2,
+ 5,
+ // f(x) = x²
+ (double x) -> x * x,
+ // d²f/dx² = 2
+ (double x) -> 2.0,
+ h,
+ -20.0,
+ 20.0);
+
+ assertCentralResults(
+ 2,
+ 5,
+ // f(x) = sin(x)
+ (double x) -> Math.sin(x),
+ // d²f/dx² = -sin(x)
+ (double x) -> -Math.sin(x),
+ h,
+ -20.0,
+ 20.0);
+
+ assertCentralResults(
+ 2,
+ 5,
+ // f(x) = ln(x)
+ (double x) -> Math.log(x),
+ // d²f/dx² = -1 / x²
+ (double x) -> -1.0 / (x * x),
+ h,
+ 1.0,
+ 20.0);
+ }
+
/** Test backward finite difference. */
@Test
void backwardFiniteDifferenceTest() {
double h = 0.005;
- assertResults(
+ assertBackwardResults(
1,
2,
// f(x) = x²
@@ -125,7 +197,7 @@
-20.0,
20.0);
- assertResults(
+ assertBackwardResults(
1,
2,
// f(x) = sin(x)
@@ -136,7 +208,7 @@
-20.0,
20.0);
- assertResults(
+ assertBackwardResults(
1,
2,
// f(x) = ln(x)
@@ -147,7 +219,7 @@
1.0,
20.0);
- assertResults(
+ assertBackwardResults(
2,
4,
// f(x) = x²
@@ -158,7 +230,7 @@
-20.0,
20.0);
- assertResults(
+ assertBackwardResults(
2,
4,
// f(x) = sin(x)
@@ -169,7 +241,7 @@
-20.0,
20.0);
- assertResults(
+ assertBackwardResults(
2,
4,
// f(x) = ln(x)
@@ -182,6 +254,53 @@
}
/**
+ * Helper for checking results of central finite difference.
+ *
+ * @param derivative The order of the derivative.
+ * @param samples The number of sample points.
+ * @param f Function of which to take derivative.
+ * @param dfdx Derivative of f.
+ * @param h Sample period in seconds.
+ * @param min Minimum of f's domain to test.
+ * @param max Maximum of f's domain to test.
+ */
+ void assertCentralResults(
+ int derivative,
+ int samples,
+ DoubleFunction<Double> f,
+ DoubleFunction<Double> dfdx,
+ double h,
+ double min,
+ double max) {
+ if (samples % 2 == 0) {
+ throw new IllegalArgumentException("Number of samples must be odd.");
+ }
+
+ // Generate stencil points from -(samples - 1)/2 to (samples - 1)/2
+ int[] stencil = new int[samples];
+ for (int i = 0; i < samples; ++i) {
+ stencil[i] = -(samples - 1) / 2 + i;
+ }
+
+ var filter = LinearFilter.finiteDifference(derivative, samples, stencil, h);
+
+ for (int i = (int) (min / h); i < (int) (max / h); ++i) {
+ // Let filter initialize
+ if (i < (int) (min / h) + samples) {
+ filter.calculate(f.apply(i * h));
+ continue;
+ }
+
+ // The order of accuracy is O(h^(N - d)) where N is number of stencil
+ // points and d is order of derivative
+ assertEquals(
+ dfdx.apply((i - samples / 2) * h),
+ filter.calculate(f.apply(i * h)),
+ Math.pow(h, samples - derivative));
+ }
+ }
+
+ /**
* Helper for checking results of backward finite difference.
*
* @param derivative The order of the derivative.
@@ -192,7 +311,7 @@
* @param min Minimum of f's domain to test.
* @param max Maximum of f's domain to test.
*/
- void assertResults(
+ void assertBackwardResults(
int derivative,
int samples,
DoubleFunction<Double> f,
@@ -209,6 +328,8 @@
continue;
}
+ // For central finite difference, the derivative computed at this point is
+ // half the window size in the past.
// The order of accuracy is O(h^(N - d)) where N is number of stencil
// points and d is order of derivative
assertEquals(
diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/MedianFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/MedianFilterTest.java
index 06b3d01..1d6e008 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/filter/MedianFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/filter/MedianFilterTest.java
@@ -8,7 +8,7 @@
import org.junit.jupiter.api.Test;
-public class MedianFilterTest {
+class MedianFilterTest {
@Test
void medianFilterNotFullTestEven() {
MedianFilter filter = new MedianFilter(10);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java
index 9c1f2f3..e7ec644 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java
@@ -8,28 +8,34 @@
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.util.WPIUtilJNI;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
-public class SlewRateLimiterTest {
+class SlewRateLimiterTest {
+ @BeforeEach
+ void setUp() {
+ WPIUtilJNI.enableMockTime();
+ WPIUtilJNI.setMockTime(0L);
+ }
+
+ @AfterEach
+ void tearDown() {
+ WPIUtilJNI.setMockTime(0L);
+ WPIUtilJNI.disableMockTime();
+ }
+
@Test
void slewRateLimitTest() {
- WPIUtilJNI.enableMockTime();
-
var limiter = new SlewRateLimiter(1);
WPIUtilJNI.setMockTime(1000000L);
assertTrue(limiter.calculate(2) < 2);
-
- WPIUtilJNI.setMockTime(0L);
}
@Test
void slewRateNoLimitTest() {
- WPIUtilJNI.enableMockTime();
-
var limiter = new SlewRateLimiter(1);
WPIUtilJNI.setMockTime(1000000L);
assertEquals(limiter.calculate(0.5), 0.5);
-
- WPIUtilJNI.setMockTime(0L);
}
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
index b6e66af..5a314b1 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
@@ -53,6 +53,7 @@
assertNotEquals(one, two);
}
+ @Test
void testMinus() {
var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java
index c13bb09..69a4c86 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java
@@ -60,6 +60,7 @@
assertNotEquals(one, two);
}
+ @Test
void testPose2dLog() {
final var start = new Pose2d();
final var end = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
new file mode 100644
index 0000000..7b0e334
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.junit.jupiter.api.Test;
+
+class TimeInterpolatableBufferTest {
+ @Test
+ void testInterpolation() {
+ TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
+
+ buffer.addSample(0, new Rotation2d());
+ assertEquals(0, buffer.getSample(0).getRadians(), 0.001);
+ buffer.addSample(1, new Rotation2d(1));
+ assertEquals(0.5, buffer.getSample(0.5).getRadians(), 0.001);
+ assertEquals(1.0, buffer.getSample(1.0).getRadians(), 0.001);
+ buffer.addSample(3, new Rotation2d(2));
+ assertEquals(1.5, buffer.getSample(2).getRadians(), 0.001);
+
+ buffer.addSample(10.5, new Rotation2d(2));
+ assertEquals(new Rotation2d(1), buffer.getSample(0));
+ }
+
+ @Test
+ void testPose2d() {
+ TimeInterpolatableBuffer<Pose2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
+
+ // We expect to be at (1 - 1/Math.sqrt(2), 1/Math.sqrt(2), 45deg) at t=0.5
+ buffer.addSample(0, new Pose2d(0, 0, Rotation2d.fromDegrees(90)));
+ buffer.addSample(1, new Pose2d(1, 1, Rotation2d.fromDegrees(0)));
+ Pose2d sample = buffer.getSample(0.5);
+
+ assertEquals(1 - 1 / Math.sqrt(2), sample.getTranslation().getX(), 0.01);
+ assertEquals(1 / Math.sqrt(2), sample.getTranslation().getY(), 0.01);
+ assertEquals(45, sample.getRotation().getDegrees(), 0.01);
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
index d334679..b3546f5 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
@@ -160,9 +160,9 @@
}
@Test
- void testNormalize() {
+ void testDesaturate() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
- wheelSpeeds.normalize(5.5);
+ wheelSpeeds.desaturate(5.5);
double factor = 5.5 / 7.0;
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
index eec6aa2..574e086 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
@@ -229,14 +229,14 @@
}
@Test
- void testNormalize() {
+ void testDesaturate() {
SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d());
SwerveModuleState br = new SwerveModuleState(7, new Rotation2d());
SwerveModuleState[] arr = {fl, fr, bl, br};
- SwerveDriveKinematics.normalizeWheelSpeeds(arr, 5.5);
+ SwerveDriveKinematics.desaturateWheelSpeeds(arr, 5.5);
double factor = 5.5 / 7.0;
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
index add1afb..3dff90b 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
@@ -14,11 +14,11 @@
import edu.wpi.first.math.numbers.N2;
import org.junit.jupiter.api.Test;
-public class DiscretizationTest {
+class DiscretizationTest {
// Check that for a simple second-order system that we can easily analyze
// analytically,
@Test
- public void testDiscretizeA() {
+ void testDiscretizeA() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
final var x0 = VecBuilder.fill(1, 1);
@@ -36,7 +36,7 @@
// Check that for a simple second-order system that we can easily analyze
// analytically,
@Test
- public void testDiscretizeAB() {
+ void testDiscretizeAB() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
final var contB = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0, 1);
@@ -62,7 +62,7 @@
// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
@Test
- public void testDiscretizeSlowModelAQ() {
+ void testDiscretizeSlowModelAQ() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
@@ -91,7 +91,7 @@
// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
@Test
- public void testDiscretizeFastModelAQ() {
+ void testDiscretizeFastModelAQ() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1406.29);
final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
@@ -118,7 +118,7 @@
// Test that the Taylor series discretization produces nearly identical results.
@Test
- public void testDiscretizeSlowModelAQTaylor() {
+ void testDiscretizeSlowModelAQTaylor() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
@@ -161,7 +161,7 @@
// Test that the Taylor series discretization produces nearly identical results.
@Test
- public void testDiscretizeFastModelAQTaylor() {
+ void testDiscretizeFastModelAQTaylor() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1500);
final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
@@ -204,7 +204,7 @@
// Test that DiscretizeR() works
@Test
- public void testDiscretizeR() {
+ void testDiscretizeR() {
var contR = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 0.0, 0.0, 1.0);
var discRTruth = Matrix.mat(Nat.N2(), Nat.N2()).fill(4.0, 0.0, 0.0, 2.0);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java
index 37cb1ec..3f316b3 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java
@@ -16,7 +16,7 @@
class LinearSystemIDTest {
@Test
- public void testDrivetrainVelocitySystem() {
+ void testDrivetrainVelocitySystem() {
var model =
LinearSystemId.createDrivetrainVelocitySystem(DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6);
assertTrue(
@@ -40,7 +40,7 @@
}
@Test
- public void testElevatorSystem() {
+ void testElevatorSystem() {
var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12);
assertTrue(
model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -99.05473), 0.001));
@@ -53,7 +53,7 @@
}
@Test
- public void testFlywheelSystem() {
+ void testFlywheelSystem() {
var model = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00032, 1.0);
assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001));
@@ -65,7 +65,20 @@
}
@Test
- public void testIdentifyPositionSystem() {
+ void testDCMotorSystem() {
+ var model = LinearSystemId.createDCMotorSystem(DCMotor.getNEO(2), 0.00032, 1.0);
+ assertTrue(
+ model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -26.87032), 0.001));
+
+ assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1354.166667), 0.001));
+
+ assertTrue(model.getC().isEqual(Matrix.eye(Nat.N2()), 0.001));
+
+ assertTrue(model.getD().isEqual(new Matrix<>(Nat.N2(), Nat.N1()), 0.001));
+ }
+
+ @Test
+ void testIdentifyPositionSystem() {
// By controls engineering in frc,
// x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
var kv = 1.0;
@@ -77,7 +90,7 @@
}
@Test
- public void testIdentifyVelocitySystem() {
+ void testIdentifyVelocitySystem() {
// By controls engineering in frc,
// V = kv * velocity + ka * acceleration
// x-dot = -kv/ka * v + 1/ka \cdot V
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
index b3fe7e6..6671963 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
@@ -12,9 +12,9 @@
import edu.wpi.first.math.numbers.N1;
import org.junit.jupiter.api.Test;
-public class NumericalIntegrationTest {
+class NumericalIntegrationTest {
@Test
- public void testExponential() {
+ void testExponential() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
var y1 =
@@ -31,7 +31,7 @@
}
@Test
- public void testExponentialRKF45() {
+ void testExponentialRKF45() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
var y1 =
@@ -49,7 +49,7 @@
}
@Test
- public void testExponentialRKDP() {
+ void testExponentialRKDP() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
var y1 =
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
index ee843ab..e4df93f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
@@ -12,7 +12,7 @@
import edu.wpi.first.math.numbers.N1;
import org.junit.jupiter.api.Test;
-public class RungeKuttaTimeVaryingTest {
+class RungeKuttaTimeVaryingTest {
private static Matrix<N1, N1> rungeKuttaTimeVaryingSolution(double t) {
return new MatBuilder<>(Nat.N1(), Nat.N1())
.fill(12.0 * Math.exp(t) / (Math.pow(Math.exp(t) + 1.0, 2.0)));
@@ -26,7 +26,7 @@
//
// x(t) = 12 * e^t / ((e^t + 1)^2)
@Test
- public void rungeKuttaTimeVaryingTest() {
+ void rungeKuttaTimeVaryingTest() {
final var y0 = rungeKuttaTimeVaryingSolution(5.0);
final var y1 =
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java
index f9e3c18..039b3f7 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java
@@ -16,7 +16,7 @@
import java.util.List;
import org.junit.jupiter.api.Test;
-public class EllipticalRegionConstraintTest {
+class EllipticalRegionConstraintTest {
@Test
void testConstraint() {
// Create constraints
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java
index 1ab826e..390a6c2 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java
@@ -16,7 +16,7 @@
import java.util.List;
import org.junit.jupiter.api.Test;
-public class RectangularRegionConstraintTest {
+class RectangularRegionConstraintTest {
@Test
void testConstraint() {
// Create constraints
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryJsonTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryJsonTest.java
index bb72601..3c6627f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryJsonTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryJsonTest.java
@@ -12,11 +12,11 @@
import java.util.List;
import org.junit.jupiter.api.Test;
-public class TrajectoryJsonTest {
+class TrajectoryJsonTest {
@Test
void deserializeMatches() {
var config =
- List.of(new DifferentialDriveKinematicsConstraint(new DifferentialDriveKinematics(20), 3));
+ List.of(new DifferentialDriveKinematicsConstraint(new DifferentialDriveKinematics(0.5), 3));
var trajectory = TrajectoryGeneratorTest.getTrajectory(config);
var deserialized =
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
index 062563f..ee6cc8f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
@@ -201,7 +201,7 @@
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
- if (!reachedGoal && (Math.abs(state.velocity - 1) < 10e-5)) {
+ if (!reachedGoal && Math.abs(state.velocity - 1) < 10e-5) {
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
reachedGoal = true;
}
@@ -244,7 +244,7 @@
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
- if (!reachedGoal && (Math.abs(state.velocity + 1) < 10e-5)) {
+ if (!reachedGoal && Math.abs(state.velocity + 1) < 10e-5) {
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
reachedGoal = true;
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/util/UnitsTest.java b/wpimath/src/test/java/edu/wpi/first/math/util/UnitsTest.java
index 99a8b5a..06c2bc7 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/util/UnitsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/util/UnitsTest.java
@@ -64,6 +64,7 @@
assertEquals(1500, Units.secondsToMilliseconds(1.5), 1e-2);
}
+ @Test
void kilogramsToLbsTest() {
assertEquals(2.20462, Units.kilogramsToLbs(1), 1e-2);
}
diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
index 8eed93f..0501f6d 100644
--- a/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
@@ -34,19 +34,20 @@
}
@Test
- public void singleConstructorTest() {
+ void singleConstructorTest() {
assertEquals(1, m_clazz.getDeclaredConstructors().length, "More than one constructor defined");
}
@Test
- public void constructorPrivateTest() {
+ void constructorPrivateTest() {
Constructor<?> constructor = m_clazz.getDeclaredConstructors()[0];
assertFalse(constructor.canAccess(null), "Constructor is not private");
}
@Test
- public void constructorReflectionTest() {
+ @SuppressWarnings("PMD.AvoidAccessibilityAlteration")
+ void constructorReflectionTest() {
Constructor<?> constructor = m_clazz.getDeclaredConstructors()[0];
constructor.setAccessible(true);
assertThrows(InvocationTargetException.class, constructor::newInstance);
diff --git a/wpimath/src/test/native/cpp/UnitsTest.cpp b/wpimath/src/test/native/cpp/UnitsTest.cpp
index ec38a47..d5ba717 100644
--- a/wpimath/src/test/native/cpp/UnitsTest.cpp
+++ b/wpimath/src/test/native/cpp/UnitsTest.cpp
@@ -164,8 +164,8 @@
} // namespace
TEST_F(TypeTraits, isRatio) {
- EXPECT_TRUE(traits::is_ratio<std::ratio<1>>::value);
- EXPECT_FALSE(traits::is_ratio<double>::value);
+ EXPECT_TRUE(traits::is_ratio_v<std::ratio<1>>);
+ EXPECT_FALSE(traits::is_ratio_v<double>);
}
TEST_F(TypeTraits, ratio_sqrt) {
@@ -210,77 +210,77 @@
5e-9);
}
-TEST_F(TypeTraits, is_unit) {
- EXPECT_FALSE(traits::is_unit<std::ratio<1>>::value);
- EXPECT_FALSE(traits::is_unit<double>::value);
- EXPECT_TRUE(traits::is_unit<meters>::value);
- EXPECT_TRUE(traits::is_unit<feet>::value);
- EXPECT_TRUE(traits::is_unit<degrees_squared>::value);
- EXPECT_FALSE(traits::is_unit<meter_t>::value);
+TEST_F(TypeTraits, is_unit_v) {
+ EXPECT_FALSE(traits::is_unit_v<std::ratio<1>>);
+ EXPECT_FALSE(traits::is_unit_v<double>);
+ EXPECT_TRUE(traits::is_unit_v<meters>);
+ EXPECT_TRUE(traits::is_unit_v<feet>);
+ EXPECT_TRUE(traits::is_unit_v<degrees_squared>);
+ EXPECT_FALSE(traits::is_unit_v<meter_t>);
}
TEST_F(TypeTraits, is_unit_t) {
- EXPECT_FALSE(traits::is_unit_t<std::ratio<1>>::value);
- EXPECT_FALSE(traits::is_unit_t<double>::value);
- EXPECT_FALSE(traits::is_unit_t<meters>::value);
- EXPECT_FALSE(traits::is_unit_t<feet>::value);
- EXPECT_FALSE(traits::is_unit_t<degrees_squared>::value);
- EXPECT_TRUE(traits::is_unit_t<meter_t>::value);
+ EXPECT_FALSE(traits::is_unit_t_v<std::ratio<1>>);
+ EXPECT_FALSE(traits::is_unit_t_v<double>);
+ EXPECT_FALSE(traits::is_unit_t_v<meters>);
+ EXPECT_FALSE(traits::is_unit_t_v<feet>);
+ EXPECT_FALSE(traits::is_unit_t_v<degrees_squared>);
+ EXPECT_TRUE(traits::is_unit_t_v<meter_t>);
}
TEST_F(TypeTraits, unit_traits) {
EXPECT_TRUE(
- (std::is_same<void,
- traits::unit_traits<double>::conversion_ratio>::value));
- EXPECT_FALSE(
- (std::is_same<void,
- traits::unit_traits<meters>::conversion_ratio>::value));
+ (std::is_same_v<void,
+ traits::unit_traits<double>::conversion_ratio>));
+ EXPECT_TRUE(
+ !(std::is_same_v<void,
+ traits::unit_traits<meters>::conversion_ratio>));
}
TEST_F(TypeTraits, unit_t_traits) {
EXPECT_TRUE(
- (std::is_same<void,
- traits::unit_t_traits<double>::underlying_type>::value));
+ (std::is_same_v<void,
+ traits::unit_t_traits<double>::underlying_type>));
EXPECT_TRUE(
- (std::is_same<UNIT_LIB_DEFAULT_TYPE,
- traits::unit_t_traits<meter_t>::underlying_type>::value));
+ (std::is_same_v<UNIT_LIB_DEFAULT_TYPE,
+ traits::unit_t_traits<meter_t>::underlying_type>));
EXPECT_TRUE(
- (std::is_same<void, traits::unit_t_traits<double>::value_type>::value));
+ (std::is_same_v<void, traits::unit_t_traits<double>::value_type>));
EXPECT_TRUE(
- (std::is_same<UNIT_LIB_DEFAULT_TYPE,
- traits::unit_t_traits<meter_t>::value_type>::value));
+ (std::is_same_v<UNIT_LIB_DEFAULT_TYPE,
+ traits::unit_t_traits<meter_t>::value_type>));
}
TEST_F(TypeTraits, all_true) {
- EXPECT_TRUE(all_true<true>::type::value);
- EXPECT_TRUE((all_true<true, true>::type::value));
- EXPECT_TRUE((all_true<true, true, true>::type::value));
- EXPECT_FALSE(all_true<false>::type::value);
- EXPECT_FALSE((all_true<true, false>::type::value));
- EXPECT_FALSE((all_true<true, true, false>::type::value));
- EXPECT_FALSE((all_true<false, false, false>::type::value));
+ EXPECT_TRUE(all_true_t_v<true>);
+ EXPECT_TRUE((all_true_t_v<true, true>));
+ EXPECT_TRUE((all_true_t_v<true, true, true>));
+ EXPECT_FALSE(all_true_t_v<false>);
+ EXPECT_FALSE((all_true_t_v<true, false>));
+ EXPECT_FALSE((all_true_t_v<true, true, false>));
+ EXPECT_FALSE((all_true_t_v<false, false, false>));
}
TEST_F(TypeTraits, is_convertible_unit) {
- EXPECT_TRUE((traits::is_convertible_unit<meters, meters>::value));
- EXPECT_TRUE((traits::is_convertible_unit<meters, astronicalUnits>::value));
- EXPECT_TRUE((traits::is_convertible_unit<meters, parsecs>::value));
+ EXPECT_TRUE((traits::is_convertible_unit_v<meters, meters>));
+ EXPECT_TRUE((traits::is_convertible_unit_v<meters, astronicalUnits>));
+ EXPECT_TRUE((traits::is_convertible_unit_v<meters, parsecs>));
- EXPECT_TRUE((traits::is_convertible_unit<meters, meters>::value));
- EXPECT_TRUE((traits::is_convertible_unit<astronicalUnits, meters>::value));
- EXPECT_TRUE((traits::is_convertible_unit<parsecs, meters>::value));
- EXPECT_TRUE((traits::is_convertible_unit<years, weeks>::value));
+ EXPECT_TRUE((traits::is_convertible_unit_v<meters, meters>));
+ EXPECT_TRUE((traits::is_convertible_unit_v<astronicalUnits, meters>));
+ EXPECT_TRUE((traits::is_convertible_unit_v<parsecs, meters>));
+ EXPECT_TRUE((traits::is_convertible_unit_v<years, weeks>));
- EXPECT_FALSE((traits::is_convertible_unit<meters, seconds>::value));
- EXPECT_FALSE((traits::is_convertible_unit<seconds, meters>::value));
- EXPECT_FALSE((traits::is_convertible_unit<years, meters>::value));
+ EXPECT_FALSE((traits::is_convertible_unit_v<meters, seconds>));
+ EXPECT_FALSE((traits::is_convertible_unit_v<seconds, meters>));
+ EXPECT_FALSE((traits::is_convertible_unit_v<years, meters>));
}
TEST_F(TypeTraits, inverse) {
double test;
using htz = inverse<seconds>;
- bool shouldBeTrue = std::is_same<htz, hertz>::value;
+ bool shouldBeTrue = std::is_same_v<htz, hertz>;
EXPECT_TRUE(shouldBeTrue);
test = convert<inverse<celsius>, inverse<fahrenheit>>(1.0);
@@ -292,564 +292,564 @@
TEST_F(TypeTraits, base_unit_of) {
using base = traits::base_unit_of<years>;
- bool shouldBeTrue = std::is_same<base, category::time_unit>::value;
+ bool shouldBeTrue = std::is_same_v<base, category::time_unit>;
EXPECT_TRUE(shouldBeTrue);
}
TEST_F(TypeTraits, has_linear_scale) {
- EXPECT_TRUE((traits::has_linear_scale<scalar_t>::value));
- EXPECT_TRUE((traits::has_linear_scale<meter_t>::value));
- EXPECT_TRUE((traits::has_linear_scale<foot_t>::value));
- EXPECT_TRUE((traits::has_linear_scale<watt_t, scalar_t>::value));
- EXPECT_TRUE((traits::has_linear_scale<scalar_t, meter_t>::value));
- EXPECT_TRUE((traits::has_linear_scale<meters_per_second_t>::value));
- EXPECT_FALSE((traits::has_linear_scale<dB_t>::value));
- EXPECT_FALSE((traits::has_linear_scale<dB_t, meters_per_second_t>::value));
+ EXPECT_TRUE((traits::has_linear_scale_v<scalar_t>));
+ EXPECT_TRUE((traits::has_linear_scale_v<meter_t>));
+ EXPECT_TRUE((traits::has_linear_scale_v<foot_t>));
+ EXPECT_TRUE((traits::has_linear_scale_v<watt_t, scalar_t>));
+ EXPECT_TRUE((traits::has_linear_scale_v<scalar_t, meter_t>));
+ EXPECT_TRUE((traits::has_linear_scale_v<meters_per_second_t>));
+ EXPECT_FALSE((traits::has_linear_scale_v<dB_t>));
+ EXPECT_FALSE((traits::has_linear_scale_v<dB_t, meters_per_second_t>));
}
TEST_F(TypeTraits, has_decibel_scale) {
- EXPECT_FALSE((traits::has_decibel_scale<scalar_t>::value));
- EXPECT_FALSE((traits::has_decibel_scale<meter_t>::value));
- EXPECT_FALSE((traits::has_decibel_scale<foot_t>::value));
- EXPECT_TRUE((traits::has_decibel_scale<dB_t>::value));
- EXPECT_TRUE((traits::has_decibel_scale<dBW_t>::value));
+ EXPECT_FALSE((traits::has_decibel_scale_v<scalar_t>));
+ EXPECT_FALSE((traits::has_decibel_scale_v<meter_t>));
+ EXPECT_FALSE((traits::has_decibel_scale_v<foot_t>));
+ EXPECT_TRUE((traits::has_decibel_scale_v<dB_t>));
+ EXPECT_TRUE((traits::has_decibel_scale_v<dBW_t>));
- EXPECT_TRUE((traits::has_decibel_scale<dBW_t, dB_t>::value));
- EXPECT_TRUE((traits::has_decibel_scale<dBW_t, dBm_t>::value));
- EXPECT_TRUE((traits::has_decibel_scale<dB_t, dB_t>::value));
- EXPECT_TRUE((traits::has_decibel_scale<dB_t, dB_t, dB_t>::value));
- EXPECT_FALSE((traits::has_decibel_scale<dB_t, dB_t, meter_t>::value));
- EXPECT_FALSE((traits::has_decibel_scale<meter_t, dB_t>::value));
+ EXPECT_TRUE((traits::has_decibel_scale_v<dBW_t, dB_t>));
+ EXPECT_TRUE((traits::has_decibel_scale_v<dBW_t, dBm_t>));
+ EXPECT_TRUE((traits::has_decibel_scale_v<dB_t, dB_t>));
+ EXPECT_TRUE((traits::has_decibel_scale_v<dB_t, dB_t, dB_t>));
+ EXPECT_FALSE((traits::has_decibel_scale_v<dB_t, dB_t, meter_t>));
+ EXPECT_FALSE((traits::has_decibel_scale_v<meter_t, dB_t>));
}
TEST_F(TypeTraits, is_same_scale) {
- EXPECT_TRUE((traits::is_same_scale<scalar_t, dimensionless_t>::value));
- EXPECT_TRUE((traits::is_same_scale<dB_t, dBW_t>::value));
- EXPECT_FALSE((traits::is_same_scale<dB_t, scalar_t>::value));
+ EXPECT_TRUE((traits::is_same_scale_v<scalar_t, dimensionless_t>));
+ EXPECT_TRUE((traits::is_same_scale_v<dB_t, dBW_t>));
+ EXPECT_FALSE((traits::is_same_scale_v<dB_t, scalar_t>));
}
TEST_F(TypeTraits, is_dimensionless_unit) {
- EXPECT_TRUE((traits::is_dimensionless_unit<scalar_t>::value));
- EXPECT_TRUE((traits::is_dimensionless_unit<const scalar_t>::value));
- EXPECT_TRUE((traits::is_dimensionless_unit<const scalar_t&>::value));
- EXPECT_TRUE((traits::is_dimensionless_unit<dimensionless_t>::value));
- EXPECT_TRUE((traits::is_dimensionless_unit<dB_t>::value));
- EXPECT_TRUE((traits::is_dimensionless_unit<dB_t, scalar_t>::value));
- EXPECT_TRUE((traits::is_dimensionless_unit<ppm_t>::value));
- EXPECT_FALSE((traits::is_dimensionless_unit<meter_t>::value));
- EXPECT_FALSE((traits::is_dimensionless_unit<dBW_t>::value));
- EXPECT_FALSE((traits::is_dimensionless_unit<dBW_t, scalar_t>::value));
+ EXPECT_TRUE((traits::is_dimensionless_unit_v<scalar_t>));
+ EXPECT_TRUE((traits::is_dimensionless_unit_v<const scalar_t>));
+ EXPECT_TRUE((traits::is_dimensionless_unit_v<const scalar_t&>));
+ EXPECT_TRUE((traits::is_dimensionless_unit_v<dimensionless_t>));
+ EXPECT_TRUE((traits::is_dimensionless_unit_v<dB_t>));
+ EXPECT_TRUE((traits::is_dimensionless_unit_v<dB_t, scalar_t>));
+ EXPECT_TRUE((traits::is_dimensionless_unit_v<ppm_t>));
+ EXPECT_FALSE((traits::is_dimensionless_unit_v<meter_t>));
+ EXPECT_FALSE((traits::is_dimensionless_unit_v<dBW_t>));
+ EXPECT_FALSE((traits::is_dimensionless_unit_v<dBW_t, scalar_t>));
}
TEST_F(TypeTraits, is_length_unit) {
- EXPECT_TRUE((traits::is_length_unit<meter>::value));
- EXPECT_TRUE((traits::is_length_unit<cubit>::value));
- EXPECT_FALSE((traits::is_length_unit<year>::value));
- EXPECT_FALSE((traits::is_length_unit<double>::value));
+ EXPECT_TRUE((traits::is_length_unit_v<meter>));
+ EXPECT_TRUE((traits::is_length_unit_v<cubit>));
+ EXPECT_FALSE((traits::is_length_unit_v<year>));
+ EXPECT_FALSE((traits::is_length_unit_v<double>));
- EXPECT_TRUE((traits::is_length_unit<meter_t>::value));
- EXPECT_TRUE((traits::is_length_unit<const meter_t>::value));
- EXPECT_TRUE((traits::is_length_unit<const meter_t&>::value));
- EXPECT_TRUE((traits::is_length_unit<cubit_t>::value));
- EXPECT_FALSE((traits::is_length_unit<year_t>::value));
- EXPECT_TRUE((traits::is_length_unit<meter_t, cubit_t>::value));
- EXPECT_FALSE((traits::is_length_unit<meter_t, year_t>::value));
+ EXPECT_TRUE((traits::is_length_unit_v<meter_t>));
+ EXPECT_TRUE((traits::is_length_unit_v<const meter_t>));
+ EXPECT_TRUE((traits::is_length_unit_v<const meter_t&>));
+ EXPECT_TRUE((traits::is_length_unit_v<cubit_t>));
+ EXPECT_FALSE((traits::is_length_unit_v<year_t>));
+ EXPECT_TRUE((traits::is_length_unit_v<meter_t, cubit_t>));
+ EXPECT_FALSE((traits::is_length_unit_v<meter_t, year_t>));
}
TEST_F(TypeTraits, is_mass_unit) {
- EXPECT_TRUE((traits::is_mass_unit<kilogram>::value));
- EXPECT_TRUE((traits::is_mass_unit<stone>::value));
- EXPECT_FALSE((traits::is_mass_unit<meter>::value));
- EXPECT_FALSE((traits::is_mass_unit<double>::value));
+ EXPECT_TRUE((traits::is_mass_unit_v<kilogram>));
+ EXPECT_TRUE((traits::is_mass_unit_v<stone>));
+ EXPECT_FALSE((traits::is_mass_unit_v<meter>));
+ EXPECT_FALSE((traits::is_mass_unit_v<double>));
- EXPECT_TRUE((traits::is_mass_unit<kilogram_t>::value));
- EXPECT_TRUE((traits::is_mass_unit<const kilogram_t>::value));
- EXPECT_TRUE((traits::is_mass_unit<const kilogram_t&>::value));
- EXPECT_TRUE((traits::is_mass_unit<stone_t>::value));
- EXPECT_FALSE((traits::is_mass_unit<meter_t>::value));
- EXPECT_TRUE((traits::is_mass_unit<kilogram_t, stone_t>::value));
- EXPECT_FALSE((traits::is_mass_unit<kilogram_t, meter_t>::value));
+ EXPECT_TRUE((traits::is_mass_unit_v<kilogram_t>));
+ EXPECT_TRUE((traits::is_mass_unit_v<const kilogram_t>));
+ EXPECT_TRUE((traits::is_mass_unit_v<const kilogram_t&>));
+ EXPECT_TRUE((traits::is_mass_unit_v<stone_t>));
+ EXPECT_FALSE((traits::is_mass_unit_v<meter_t>));
+ EXPECT_TRUE((traits::is_mass_unit_v<kilogram_t, stone_t>));
+ EXPECT_FALSE((traits::is_mass_unit_v<kilogram_t, meter_t>));
}
TEST_F(TypeTraits, is_time_unit) {
- EXPECT_TRUE((traits::is_time_unit<second>::value));
- EXPECT_TRUE((traits::is_time_unit<year>::value));
- EXPECT_FALSE((traits::is_time_unit<meter>::value));
- EXPECT_FALSE((traits::is_time_unit<double>::value));
+ EXPECT_TRUE((traits::is_time_unit_v<second>));
+ EXPECT_TRUE((traits::is_time_unit_v<year>));
+ EXPECT_FALSE((traits::is_time_unit_v<meter>));
+ EXPECT_FALSE((traits::is_time_unit_v<double>));
- EXPECT_TRUE((traits::is_time_unit<second_t>::value));
- EXPECT_TRUE((traits::is_time_unit<const second_t>::value));
- EXPECT_TRUE((traits::is_time_unit<const second_t&>::value));
- EXPECT_TRUE((traits::is_time_unit<year_t>::value));
- EXPECT_FALSE((traits::is_time_unit<meter_t>::value));
- EXPECT_TRUE((traits::is_time_unit<second_t, year_t>::value));
- EXPECT_FALSE((traits::is_time_unit<second_t, meter_t>::value));
+ EXPECT_TRUE((traits::is_time_unit_v<second_t>));
+ EXPECT_TRUE((traits::is_time_unit_v<const second_t>));
+ EXPECT_TRUE((traits::is_time_unit_v<const second_t&>));
+ EXPECT_TRUE((traits::is_time_unit_v<year_t>));
+ EXPECT_FALSE((traits::is_time_unit_v<meter_t>));
+ EXPECT_TRUE((traits::is_time_unit_v<second_t, year_t>));
+ EXPECT_FALSE((traits::is_time_unit_v<second_t, meter_t>));
}
TEST_F(TypeTraits, is_angle_unit) {
- EXPECT_TRUE((traits::is_angle_unit<angle::radian>::value));
- EXPECT_TRUE((traits::is_angle_unit<angle::degree>::value));
- EXPECT_FALSE((traits::is_angle_unit<watt>::value));
- EXPECT_FALSE((traits::is_angle_unit<double>::value));
+ EXPECT_TRUE((traits::is_angle_unit_v<angle::radian>));
+ EXPECT_TRUE((traits::is_angle_unit_v<angle::degree>));
+ EXPECT_FALSE((traits::is_angle_unit_v<watt>));
+ EXPECT_FALSE((traits::is_angle_unit_v<double>));
- EXPECT_TRUE((traits::is_angle_unit<angle::radian_t>::value));
- EXPECT_TRUE((traits::is_angle_unit<const angle::radian_t>::value));
- EXPECT_TRUE((traits::is_angle_unit<const angle::radian_t&>::value));
- EXPECT_TRUE((traits::is_angle_unit<angle::degree_t>::value));
- EXPECT_FALSE((traits::is_angle_unit<watt_t>::value));
- EXPECT_TRUE((traits::is_angle_unit<angle::radian_t, angle::degree_t>::value));
- EXPECT_FALSE((traits::is_angle_unit<angle::radian_t, watt_t>::value));
+ EXPECT_TRUE((traits::is_angle_unit_v<angle::radian_t>));
+ EXPECT_TRUE((traits::is_angle_unit_v<const angle::radian_t>));
+ EXPECT_TRUE((traits::is_angle_unit_v<const angle::radian_t&>));
+ EXPECT_TRUE((traits::is_angle_unit_v<angle::degree_t>));
+ EXPECT_FALSE((traits::is_angle_unit_v<watt_t>));
+ EXPECT_TRUE((traits::is_angle_unit_v<angle::radian_t, angle::degree_t>));
+ EXPECT_FALSE((traits::is_angle_unit_v<angle::radian_t, watt_t>));
}
TEST_F(TypeTraits, is_current_unit) {
- EXPECT_TRUE((traits::is_current_unit<current::ampere>::value));
- EXPECT_FALSE((traits::is_current_unit<volt>::value));
- EXPECT_FALSE((traits::is_current_unit<double>::value));
+ EXPECT_TRUE((traits::is_current_unit_v<current::ampere>));
+ EXPECT_FALSE((traits::is_current_unit_v<volt>));
+ EXPECT_FALSE((traits::is_current_unit_v<double>));
- EXPECT_TRUE((traits::is_current_unit<current::ampere_t>::value));
- EXPECT_TRUE((traits::is_current_unit<const current::ampere_t>::value));
- EXPECT_TRUE((traits::is_current_unit<const current::ampere_t&>::value));
- EXPECT_FALSE((traits::is_current_unit<volt_t>::value));
- EXPECT_TRUE((traits::is_current_unit<current::ampere_t,
- current::milliampere_t>::value));
- EXPECT_FALSE((traits::is_current_unit<current::ampere_t, volt_t>::value));
+ EXPECT_TRUE((traits::is_current_unit_v<current::ampere_t>));
+ EXPECT_TRUE((traits::is_current_unit_v<const current::ampere_t>));
+ EXPECT_TRUE((traits::is_current_unit_v<const current::ampere_t&>));
+ EXPECT_FALSE((traits::is_current_unit_v<volt_t>));
+ EXPECT_TRUE((traits::is_current_unit_v<current::ampere_t,
+ current::milliampere_t>));
+ EXPECT_FALSE((traits::is_current_unit_v<current::ampere_t, volt_t>));
}
TEST_F(TypeTraits, is_temperature_unit) {
- EXPECT_TRUE((traits::is_temperature_unit<fahrenheit>::value));
- EXPECT_TRUE((traits::is_temperature_unit<kelvin>::value));
- EXPECT_FALSE((traits::is_temperature_unit<cubit>::value));
- EXPECT_FALSE((traits::is_temperature_unit<double>::value));
+ EXPECT_TRUE((traits::is_temperature_unit_v<fahrenheit>));
+ EXPECT_TRUE((traits::is_temperature_unit_v<kelvin>));
+ EXPECT_FALSE((traits::is_temperature_unit_v<cubit>));
+ EXPECT_FALSE((traits::is_temperature_unit_v<double>));
- EXPECT_TRUE((traits::is_temperature_unit<fahrenheit_t>::value));
- EXPECT_TRUE((traits::is_temperature_unit<const fahrenheit_t>::value));
- EXPECT_TRUE((traits::is_temperature_unit<const fahrenheit_t&>::value));
- EXPECT_TRUE((traits::is_temperature_unit<kelvin_t>::value));
- EXPECT_FALSE((traits::is_temperature_unit<cubit_t>::value));
- EXPECT_TRUE((traits::is_temperature_unit<fahrenheit_t, kelvin_t>::value));
- EXPECT_FALSE((traits::is_temperature_unit<cubit_t, fahrenheit_t>::value));
+ EXPECT_TRUE((traits::is_temperature_unit_v<fahrenheit_t>));
+ EXPECT_TRUE((traits::is_temperature_unit_v<const fahrenheit_t>));
+ EXPECT_TRUE((traits::is_temperature_unit_v<const fahrenheit_t&>));
+ EXPECT_TRUE((traits::is_temperature_unit_v<kelvin_t>));
+ EXPECT_FALSE((traits::is_temperature_unit_v<cubit_t>));
+ EXPECT_TRUE((traits::is_temperature_unit_v<fahrenheit_t, kelvin_t>));
+ EXPECT_FALSE((traits::is_temperature_unit_v<cubit_t, fahrenheit_t>));
}
TEST_F(TypeTraits, is_substance_unit) {
- EXPECT_TRUE((traits::is_substance_unit<substance::mol>::value));
- EXPECT_FALSE((traits::is_substance_unit<year>::value));
- EXPECT_FALSE((traits::is_substance_unit<double>::value));
+ EXPECT_TRUE((traits::is_substance_unit_v<substance::mol>));
+ EXPECT_FALSE((traits::is_substance_unit_v<year>));
+ EXPECT_FALSE((traits::is_substance_unit_v<double>));
- EXPECT_TRUE((traits::is_substance_unit<substance::mole_t>::value));
- EXPECT_TRUE((traits::is_substance_unit<const substance::mole_t>::value));
- EXPECT_TRUE((traits::is_substance_unit<const substance::mole_t&>::value));
- EXPECT_FALSE((traits::is_substance_unit<year_t>::value));
+ EXPECT_TRUE((traits::is_substance_unit_v<substance::mole_t>));
+ EXPECT_TRUE((traits::is_substance_unit_v<const substance::mole_t>));
+ EXPECT_TRUE((traits::is_substance_unit_v<const substance::mole_t&>));
+ EXPECT_FALSE((traits::is_substance_unit_v<year_t>));
EXPECT_TRUE(
- (traits::is_substance_unit<substance::mole_t, substance::mole_t>::value));
- EXPECT_FALSE((traits::is_substance_unit<year_t, substance::mole_t>::value));
+ (traits::is_substance_unit_v<substance::mole_t, substance::mole_t>));
+ EXPECT_FALSE((traits::is_substance_unit_v<year_t, substance::mole_t>));
}
TEST_F(TypeTraits, is_luminous_intensity_unit) {
- EXPECT_TRUE((traits::is_luminous_intensity_unit<candela>::value));
+ EXPECT_TRUE((traits::is_luminous_intensity_unit_v<candela>));
EXPECT_FALSE(
- (traits::is_luminous_intensity_unit<units::radiation::rad>::value));
- EXPECT_FALSE((traits::is_luminous_intensity_unit<double>::value));
+ (traits::is_luminous_intensity_unit_v<units::radiation::rad>));
+ EXPECT_FALSE((traits::is_luminous_intensity_unit_v<double>));
- EXPECT_TRUE((traits::is_luminous_intensity_unit<candela_t>::value));
- EXPECT_TRUE((traits::is_luminous_intensity_unit<const candela_t>::value));
- EXPECT_TRUE((traits::is_luminous_intensity_unit<const candela_t&>::value));
- EXPECT_FALSE((traits::is_luminous_intensity_unit<rad_t>::value));
+ EXPECT_TRUE((traits::is_luminous_intensity_unit_v<candela_t>));
+ EXPECT_TRUE((traits::is_luminous_intensity_unit_v<const candela_t>));
+ EXPECT_TRUE((traits::is_luminous_intensity_unit_v<const candela_t&>));
+ EXPECT_FALSE((traits::is_luminous_intensity_unit_v<rad_t>));
EXPECT_TRUE(
- (traits::is_luminous_intensity_unit<candela_t, candela_t>::value));
- EXPECT_FALSE((traits::is_luminous_intensity_unit<rad_t, candela_t>::value));
+ (traits::is_luminous_intensity_unit_v<candela_t, candela_t>));
+ EXPECT_FALSE((traits::is_luminous_intensity_unit_v<rad_t, candela_t>));
}
TEST_F(TypeTraits, is_solid_angle_unit) {
- EXPECT_TRUE((traits::is_solid_angle_unit<steradian>::value));
- EXPECT_TRUE((traits::is_solid_angle_unit<degree_squared>::value));
- EXPECT_FALSE((traits::is_solid_angle_unit<angle::degree>::value));
- EXPECT_FALSE((traits::is_solid_angle_unit<double>::value));
+ EXPECT_TRUE((traits::is_solid_angle_unit_v<steradian>));
+ EXPECT_TRUE((traits::is_solid_angle_unit_v<degree_squared>));
+ EXPECT_FALSE((traits::is_solid_angle_unit_v<angle::degree>));
+ EXPECT_FALSE((traits::is_solid_angle_unit_v<double>));
- EXPECT_TRUE((traits::is_solid_angle_unit<steradian_t>::value));
- EXPECT_TRUE((traits::is_solid_angle_unit<const steradian_t>::value));
- EXPECT_TRUE((traits::is_solid_angle_unit<const degree_squared_t&>::value));
- EXPECT_FALSE((traits::is_solid_angle_unit<angle::degree_t>::value));
+ EXPECT_TRUE((traits::is_solid_angle_unit_v<steradian_t>));
+ EXPECT_TRUE((traits::is_solid_angle_unit_v<const steradian_t>));
+ EXPECT_TRUE((traits::is_solid_angle_unit_v<const degree_squared_t&>));
+ EXPECT_FALSE((traits::is_solid_angle_unit_v<angle::degree_t>));
EXPECT_TRUE(
- (traits::is_solid_angle_unit<degree_squared_t, steradian_t>::value));
+ (traits::is_solid_angle_unit_v<degree_squared_t, steradian_t>));
EXPECT_FALSE(
- (traits::is_solid_angle_unit<angle::degree_t, steradian_t>::value));
+ (traits::is_solid_angle_unit_v<angle::degree_t, steradian_t>));
}
TEST_F(TypeTraits, is_frequency_unit) {
- EXPECT_TRUE((traits::is_frequency_unit<hertz>::value));
- EXPECT_FALSE((traits::is_frequency_unit<second>::value));
- EXPECT_FALSE((traits::is_frequency_unit<double>::value));
+ EXPECT_TRUE((traits::is_frequency_unit_v<hertz>));
+ EXPECT_FALSE((traits::is_frequency_unit_v<second>));
+ EXPECT_FALSE((traits::is_frequency_unit_v<double>));
- EXPECT_TRUE((traits::is_frequency_unit<hertz_t>::value));
- EXPECT_TRUE((traits::is_frequency_unit<const hertz_t>::value));
- EXPECT_TRUE((traits::is_frequency_unit<const hertz_t&>::value));
- EXPECT_FALSE((traits::is_frequency_unit<second_t>::value));
- EXPECT_TRUE((traits::is_frequency_unit<const hertz_t&, gigahertz_t>::value));
- EXPECT_FALSE((traits::is_frequency_unit<second_t, hertz_t>::value));
+ EXPECT_TRUE((traits::is_frequency_unit_v<hertz_t>));
+ EXPECT_TRUE((traits::is_frequency_unit_v<const hertz_t>));
+ EXPECT_TRUE((traits::is_frequency_unit_v<const hertz_t&>));
+ EXPECT_FALSE((traits::is_frequency_unit_v<second_t>));
+ EXPECT_TRUE((traits::is_frequency_unit_v<const hertz_t&, gigahertz_t>));
+ EXPECT_FALSE((traits::is_frequency_unit_v<second_t, hertz_t>));
}
TEST_F(TypeTraits, is_velocity_unit) {
- EXPECT_TRUE((traits::is_velocity_unit<meters_per_second>::value));
- EXPECT_TRUE((traits::is_velocity_unit<miles_per_hour>::value));
- EXPECT_FALSE((traits::is_velocity_unit<meters_per_second_squared>::value));
- EXPECT_FALSE((traits::is_velocity_unit<double>::value));
+ EXPECT_TRUE((traits::is_velocity_unit_v<meters_per_second>));
+ EXPECT_TRUE((traits::is_velocity_unit_v<miles_per_hour>));
+ EXPECT_FALSE((traits::is_velocity_unit_v<meters_per_second_squared>));
+ EXPECT_FALSE((traits::is_velocity_unit_v<double>));
- EXPECT_TRUE((traits::is_velocity_unit<meters_per_second_t>::value));
- EXPECT_TRUE((traits::is_velocity_unit<const meters_per_second_t>::value));
- EXPECT_TRUE((traits::is_velocity_unit<const meters_per_second_t&>::value));
- EXPECT_TRUE((traits::is_velocity_unit<miles_per_hour_t>::value));
- EXPECT_FALSE((traits::is_velocity_unit<meters_per_second_squared_t>::value));
+ EXPECT_TRUE((traits::is_velocity_unit_v<meters_per_second_t>));
+ EXPECT_TRUE((traits::is_velocity_unit_v<const meters_per_second_t>));
+ EXPECT_TRUE((traits::is_velocity_unit_v<const meters_per_second_t&>));
+ EXPECT_TRUE((traits::is_velocity_unit_v<miles_per_hour_t>));
+ EXPECT_FALSE((traits::is_velocity_unit_v<meters_per_second_squared_t>));
EXPECT_TRUE(
- (traits::is_velocity_unit<miles_per_hour_t, meters_per_second_t>::value));
- EXPECT_FALSE((traits::is_velocity_unit<meters_per_second_squared_t,
- meters_per_second_t>::value));
+ (traits::is_velocity_unit_v<miles_per_hour_t, meters_per_second_t>));
+ EXPECT_FALSE((traits::is_velocity_unit_v<meters_per_second_squared_t,
+ meters_per_second_t>));
}
TEST_F(TypeTraits, is_acceleration_unit) {
- EXPECT_TRUE((traits::is_acceleration_unit<meters_per_second_squared>::value));
+ EXPECT_TRUE((traits::is_acceleration_unit_v<meters_per_second_squared>));
EXPECT_TRUE(
- (traits::is_acceleration_unit<acceleration::standard_gravity>::value));
- EXPECT_FALSE((traits::is_acceleration_unit<inch>::value));
- EXPECT_FALSE((traits::is_acceleration_unit<double>::value));
+ (traits::is_acceleration_unit_v<acceleration::standard_gravity>));
+ EXPECT_FALSE((traits::is_acceleration_unit_v<inch>));
+ EXPECT_FALSE((traits::is_acceleration_unit_v<double>));
EXPECT_TRUE(
- (traits::is_acceleration_unit<meters_per_second_squared_t>::value));
+ (traits::is_acceleration_unit_v<meters_per_second_squared_t>));
EXPECT_TRUE(
- (traits::is_acceleration_unit<const meters_per_second_squared_t>::value));
+ (traits::is_acceleration_unit_v<const meters_per_second_squared_t>));
EXPECT_TRUE((
- traits::is_acceleration_unit<const meters_per_second_squared_t&>::value));
- EXPECT_TRUE((traits::is_acceleration_unit<standard_gravity_t>::value));
- EXPECT_FALSE((traits::is_acceleration_unit<inch_t>::value));
+ traits::is_acceleration_unit_v<const meters_per_second_squared_t&>));
+ EXPECT_TRUE((traits::is_acceleration_unit_v<standard_gravity_t>));
+ EXPECT_FALSE((traits::is_acceleration_unit_v<inch_t>));
EXPECT_TRUE(
- (traits::is_acceleration_unit<standard_gravity_t,
- meters_per_second_squared_t>::value));
+ (traits::is_acceleration_unit_v<standard_gravity_t,
+ meters_per_second_squared_t>));
EXPECT_FALSE(
- (traits::is_acceleration_unit<inch_t,
- meters_per_second_squared_t>::value));
+ (traits::is_acceleration_unit_v<inch_t,
+ meters_per_second_squared_t>));
}
TEST_F(TypeTraits, is_force_unit) {
- EXPECT_TRUE((traits::is_force_unit<units::force::newton>::value));
- EXPECT_TRUE((traits::is_force_unit<units::force::dynes>::value));
- EXPECT_FALSE((traits::is_force_unit<meter>::value));
- EXPECT_FALSE((traits::is_force_unit<double>::value));
+ EXPECT_TRUE((traits::is_force_unit_v<units::force::newton>));
+ EXPECT_TRUE((traits::is_force_unit_v<units::force::dynes>));
+ EXPECT_FALSE((traits::is_force_unit_v<meter>));
+ EXPECT_FALSE((traits::is_force_unit_v<double>));
- EXPECT_TRUE((traits::is_force_unit<units::force::newton_t>::value));
- EXPECT_TRUE((traits::is_force_unit<const units::force::newton_t>::value));
- EXPECT_TRUE((traits::is_force_unit<const units::force::newton_t&>::value));
- EXPECT_TRUE((traits::is_force_unit<units::force::dyne_t>::value));
- EXPECT_FALSE((traits::is_force_unit<watt_t>::value));
- EXPECT_TRUE((traits::is_force_unit<units::force::dyne_t,
- units::force::newton_t>::value));
- EXPECT_FALSE((traits::is_force_unit<watt_t, units::force::newton_t>::value));
+ EXPECT_TRUE((traits::is_force_unit_v<units::force::newton_t>));
+ EXPECT_TRUE((traits::is_force_unit_v<const units::force::newton_t>));
+ EXPECT_TRUE((traits::is_force_unit_v<const units::force::newton_t&>));
+ EXPECT_TRUE((traits::is_force_unit_v<units::force::dyne_t>));
+ EXPECT_FALSE((traits::is_force_unit_v<watt_t>));
+ EXPECT_TRUE((traits::is_force_unit_v<units::force::dyne_t,
+ units::force::newton_t>));
+ EXPECT_FALSE((traits::is_force_unit_v<watt_t, units::force::newton_t>));
}
TEST_F(TypeTraits, is_pressure_unit) {
- EXPECT_TRUE((traits::is_pressure_unit<pressure::pascals>::value));
- EXPECT_TRUE((traits::is_pressure_unit<atmosphere>::value));
- EXPECT_FALSE((traits::is_pressure_unit<year>::value));
- EXPECT_FALSE((traits::is_pressure_unit<double>::value));
+ EXPECT_TRUE((traits::is_pressure_unit_v<pressure::pascals>));
+ EXPECT_TRUE((traits::is_pressure_unit_v<atmosphere>));
+ EXPECT_FALSE((traits::is_pressure_unit_v<year>));
+ EXPECT_FALSE((traits::is_pressure_unit_v<double>));
- EXPECT_TRUE((traits::is_pressure_unit<pascal_t>::value));
- EXPECT_TRUE((traits::is_pressure_unit<const pascal_t>::value));
- EXPECT_TRUE((traits::is_pressure_unit<const pascal_t&>::value));
- EXPECT_TRUE((traits::is_pressure_unit<atmosphere_t>::value));
- EXPECT_FALSE((traits::is_pressure_unit<year_t>::value));
+ EXPECT_TRUE((traits::is_pressure_unit_v<pascal_t>));
+ EXPECT_TRUE((traits::is_pressure_unit_v<const pascal_t>));
+ EXPECT_TRUE((traits::is_pressure_unit_v<const pascal_t&>));
+ EXPECT_TRUE((traits::is_pressure_unit_v<atmosphere_t>));
+ EXPECT_FALSE((traits::is_pressure_unit_v<year_t>));
EXPECT_TRUE(
- (traits::is_pressure_unit<atmosphere_t, pressure::pascal_t>::value));
- EXPECT_FALSE((traits::is_pressure_unit<year_t, pressure::pascal_t>::value));
+ (traits::is_pressure_unit_v<atmosphere_t, pressure::pascal_t>));
+ EXPECT_FALSE((traits::is_pressure_unit_v<year_t, pressure::pascal_t>));
}
TEST_F(TypeTraits, is_charge_unit) {
- EXPECT_TRUE((traits::is_charge_unit<coulomb>::value));
- EXPECT_FALSE((traits::is_charge_unit<watt>::value));
- EXPECT_FALSE((traits::is_charge_unit<double>::value));
+ EXPECT_TRUE((traits::is_charge_unit_v<coulomb>));
+ EXPECT_FALSE((traits::is_charge_unit_v<watt>));
+ EXPECT_FALSE((traits::is_charge_unit_v<double>));
- EXPECT_TRUE((traits::is_charge_unit<coulomb_t>::value));
- EXPECT_TRUE((traits::is_charge_unit<const coulomb_t>::value));
- EXPECT_TRUE((traits::is_charge_unit<const coulomb_t&>::value));
- EXPECT_FALSE((traits::is_charge_unit<watt_t>::value));
- EXPECT_TRUE((traits::is_charge_unit<const coulomb_t&, coulomb_t>::value));
- EXPECT_FALSE((traits::is_charge_unit<watt_t, coulomb_t>::value));
+ EXPECT_TRUE((traits::is_charge_unit_v<coulomb_t>));
+ EXPECT_TRUE((traits::is_charge_unit_v<const coulomb_t>));
+ EXPECT_TRUE((traits::is_charge_unit_v<const coulomb_t&>));
+ EXPECT_FALSE((traits::is_charge_unit_v<watt_t>));
+ EXPECT_TRUE((traits::is_charge_unit_v<const coulomb_t&, coulomb_t>));
+ EXPECT_FALSE((traits::is_charge_unit_v<watt_t, coulomb_t>));
}
TEST_F(TypeTraits, is_energy_unit) {
- EXPECT_TRUE((traits::is_energy_unit<joule>::value));
- EXPECT_TRUE((traits::is_energy_unit<calorie>::value));
- EXPECT_FALSE((traits::is_energy_unit<watt>::value));
- EXPECT_FALSE((traits::is_energy_unit<double>::value));
+ EXPECT_TRUE((traits::is_energy_unit_v<joule>));
+ EXPECT_TRUE((traits::is_energy_unit_v<calorie>));
+ EXPECT_FALSE((traits::is_energy_unit_v<watt>));
+ EXPECT_FALSE((traits::is_energy_unit_v<double>));
- EXPECT_TRUE((traits::is_energy_unit<joule_t>::value));
- EXPECT_TRUE((traits::is_energy_unit<const joule_t>::value));
- EXPECT_TRUE((traits::is_energy_unit<const joule_t&>::value));
- EXPECT_TRUE((traits::is_energy_unit<calorie_t>::value));
- EXPECT_FALSE((traits::is_energy_unit<watt_t>::value));
- EXPECT_TRUE((traits::is_energy_unit<calorie_t, joule_t>::value));
- EXPECT_FALSE((traits::is_energy_unit<watt_t, joule_t>::value));
+ EXPECT_TRUE((traits::is_energy_unit_v<joule_t>));
+ EXPECT_TRUE((traits::is_energy_unit_v<const joule_t>));
+ EXPECT_TRUE((traits::is_energy_unit_v<const joule_t&>));
+ EXPECT_TRUE((traits::is_energy_unit_v<calorie_t>));
+ EXPECT_FALSE((traits::is_energy_unit_v<watt_t>));
+ EXPECT_TRUE((traits::is_energy_unit_v<calorie_t, joule_t>));
+ EXPECT_FALSE((traits::is_energy_unit_v<watt_t, joule_t>));
}
TEST_F(TypeTraits, is_power_unit) {
- EXPECT_TRUE((traits::is_power_unit<watt>::value));
- EXPECT_FALSE((traits::is_power_unit<henry>::value));
- EXPECT_FALSE((traits::is_power_unit<double>::value));
+ EXPECT_TRUE((traits::is_power_unit_v<watt>));
+ EXPECT_FALSE((traits::is_power_unit_v<henry>));
+ EXPECT_FALSE((traits::is_power_unit_v<double>));
- EXPECT_TRUE((traits::is_power_unit<watt_t>::value));
- EXPECT_TRUE((traits::is_power_unit<const watt_t>::value));
- EXPECT_TRUE((traits::is_power_unit<const watt_t&>::value));
- EXPECT_FALSE((traits::is_power_unit<henry_t>::value));
- EXPECT_TRUE((traits::is_power_unit<const watt_t&, watt_t>::value));
- EXPECT_FALSE((traits::is_power_unit<henry_t, watt_t>::value));
+ EXPECT_TRUE((traits::is_power_unit_v<watt_t>));
+ EXPECT_TRUE((traits::is_power_unit_v<const watt_t>));
+ EXPECT_TRUE((traits::is_power_unit_v<const watt_t&>));
+ EXPECT_FALSE((traits::is_power_unit_v<henry_t>));
+ EXPECT_TRUE((traits::is_power_unit_v<const watt_t&, watt_t>));
+ EXPECT_FALSE((traits::is_power_unit_v<henry_t, watt_t>));
}
TEST_F(TypeTraits, is_voltage_unit) {
- EXPECT_TRUE((traits::is_voltage_unit<volt>::value));
- EXPECT_FALSE((traits::is_voltage_unit<henry>::value));
- EXPECT_FALSE((traits::is_voltage_unit<double>::value));
+ EXPECT_TRUE((traits::is_voltage_unit_v<volt>));
+ EXPECT_FALSE((traits::is_voltage_unit_v<henry>));
+ EXPECT_FALSE((traits::is_voltage_unit_v<double>));
- EXPECT_TRUE((traits::is_voltage_unit<volt_t>::value));
- EXPECT_TRUE((traits::is_voltage_unit<const volt_t>::value));
- EXPECT_TRUE((traits::is_voltage_unit<const volt_t&>::value));
- EXPECT_FALSE((traits::is_voltage_unit<henry_t>::value));
- EXPECT_TRUE((traits::is_voltage_unit<const volt_t&, volt_t>::value));
- EXPECT_FALSE((traits::is_voltage_unit<henry_t, volt_t>::value));
+ EXPECT_TRUE((traits::is_voltage_unit_v<volt_t>));
+ EXPECT_TRUE((traits::is_voltage_unit_v<const volt_t>));
+ EXPECT_TRUE((traits::is_voltage_unit_v<const volt_t&>));
+ EXPECT_FALSE((traits::is_voltage_unit_v<henry_t>));
+ EXPECT_TRUE((traits::is_voltage_unit_v<const volt_t&, volt_t>));
+ EXPECT_FALSE((traits::is_voltage_unit_v<henry_t, volt_t>));
}
TEST_F(TypeTraits, is_capacitance_unit) {
- EXPECT_TRUE((traits::is_capacitance_unit<farad>::value));
- EXPECT_FALSE((traits::is_capacitance_unit<ohm>::value));
- EXPECT_FALSE((traits::is_capacitance_unit<double>::value));
+ EXPECT_TRUE((traits::is_capacitance_unit_v<farad>));
+ EXPECT_FALSE((traits::is_capacitance_unit_v<ohm>));
+ EXPECT_FALSE((traits::is_capacitance_unit_v<double>));
- EXPECT_TRUE((traits::is_capacitance_unit<farad_t>::value));
- EXPECT_TRUE((traits::is_capacitance_unit<const farad_t>::value));
- EXPECT_TRUE((traits::is_capacitance_unit<const farad_t&>::value));
- EXPECT_FALSE((traits::is_capacitance_unit<ohm_t>::value));
+ EXPECT_TRUE((traits::is_capacitance_unit_v<farad_t>));
+ EXPECT_TRUE((traits::is_capacitance_unit_v<const farad_t>));
+ EXPECT_TRUE((traits::is_capacitance_unit_v<const farad_t&>));
+ EXPECT_FALSE((traits::is_capacitance_unit_v<ohm_t>));
EXPECT_TRUE(
- (traits::is_capacitance_unit<const farad_t&, millifarad_t>::value));
- EXPECT_FALSE((traits::is_capacitance_unit<ohm_t, farad_t>::value));
+ (traits::is_capacitance_unit_v<const farad_t&, millifarad_t>));
+ EXPECT_FALSE((traits::is_capacitance_unit_v<ohm_t, farad_t>));
}
TEST_F(TypeTraits, is_impedance_unit) {
- EXPECT_TRUE((traits::is_impedance_unit<ohm>::value));
- EXPECT_FALSE((traits::is_impedance_unit<farad>::value));
- EXPECT_FALSE((traits::is_impedance_unit<double>::value));
+ EXPECT_TRUE((traits::is_impedance_unit_v<ohm>));
+ EXPECT_FALSE((traits::is_impedance_unit_v<farad>));
+ EXPECT_FALSE((traits::is_impedance_unit_v<double>));
- EXPECT_TRUE((traits::is_impedance_unit<ohm_t>::value));
- EXPECT_TRUE((traits::is_impedance_unit<const ohm_t>::value));
- EXPECT_TRUE((traits::is_impedance_unit<const ohm_t&>::value));
- EXPECT_FALSE((traits::is_impedance_unit<farad_t>::value));
- EXPECT_TRUE((traits::is_impedance_unit<const ohm_t&, milliohm_t>::value));
- EXPECT_FALSE((traits::is_impedance_unit<farad_t, ohm_t>::value));
+ EXPECT_TRUE((traits::is_impedance_unit_v<ohm_t>));
+ EXPECT_TRUE((traits::is_impedance_unit_v<const ohm_t>));
+ EXPECT_TRUE((traits::is_impedance_unit_v<const ohm_t&>));
+ EXPECT_FALSE((traits::is_impedance_unit_v<farad_t>));
+ EXPECT_TRUE((traits::is_impedance_unit_v<const ohm_t&, milliohm_t>));
+ EXPECT_FALSE((traits::is_impedance_unit_v<farad_t, ohm_t>));
}
TEST_F(TypeTraits, is_conductance_unit) {
- EXPECT_TRUE((traits::is_conductance_unit<siemens>::value));
- EXPECT_FALSE((traits::is_conductance_unit<volt>::value));
- EXPECT_FALSE((traits::is_conductance_unit<double>::value));
+ EXPECT_TRUE((traits::is_conductance_unit_v<siemens>));
+ EXPECT_FALSE((traits::is_conductance_unit_v<volt>));
+ EXPECT_FALSE((traits::is_conductance_unit_v<double>));
- EXPECT_TRUE((traits::is_conductance_unit<siemens_t>::value));
- EXPECT_TRUE((traits::is_conductance_unit<const siemens_t>::value));
- EXPECT_TRUE((traits::is_conductance_unit<const siemens_t&>::value));
- EXPECT_FALSE((traits::is_conductance_unit<volt_t>::value));
+ EXPECT_TRUE((traits::is_conductance_unit_v<siemens_t>));
+ EXPECT_TRUE((traits::is_conductance_unit_v<const siemens_t>));
+ EXPECT_TRUE((traits::is_conductance_unit_v<const siemens_t&>));
+ EXPECT_FALSE((traits::is_conductance_unit_v<volt_t>));
EXPECT_TRUE(
- (traits::is_conductance_unit<const siemens_t&, millisiemens_t>::value));
- EXPECT_FALSE((traits::is_conductance_unit<volt_t, siemens_t>::value));
+ (traits::is_conductance_unit_v<const siemens_t&, millisiemens_t>));
+ EXPECT_FALSE((traits::is_conductance_unit_v<volt_t, siemens_t>));
}
TEST_F(TypeTraits, is_magnetic_flux_unit) {
- EXPECT_TRUE((traits::is_magnetic_flux_unit<weber>::value));
- EXPECT_TRUE((traits::is_magnetic_flux_unit<maxwell>::value));
- EXPECT_FALSE((traits::is_magnetic_flux_unit<inch>::value));
- EXPECT_FALSE((traits::is_magnetic_flux_unit<double>::value));
+ EXPECT_TRUE((traits::is_magnetic_flux_unit_v<weber>));
+ EXPECT_TRUE((traits::is_magnetic_flux_unit_v<maxwell>));
+ EXPECT_FALSE((traits::is_magnetic_flux_unit_v<inch>));
+ EXPECT_FALSE((traits::is_magnetic_flux_unit_v<double>));
- EXPECT_TRUE((traits::is_magnetic_flux_unit<weber_t>::value));
- EXPECT_TRUE((traits::is_magnetic_flux_unit<const weber_t>::value));
- EXPECT_TRUE((traits::is_magnetic_flux_unit<const weber_t&>::value));
- EXPECT_TRUE((traits::is_magnetic_flux_unit<maxwell_t>::value));
- EXPECT_FALSE((traits::is_magnetic_flux_unit<inch_t>::value));
- EXPECT_TRUE((traits::is_magnetic_flux_unit<maxwell_t, weber_t>::value));
- EXPECT_FALSE((traits::is_magnetic_flux_unit<inch_t, weber_t>::value));
+ EXPECT_TRUE((traits::is_magnetic_flux_unit_v<weber_t>));
+ EXPECT_TRUE((traits::is_magnetic_flux_unit_v<const weber_t>));
+ EXPECT_TRUE((traits::is_magnetic_flux_unit_v<const weber_t&>));
+ EXPECT_TRUE((traits::is_magnetic_flux_unit_v<maxwell_t>));
+ EXPECT_FALSE((traits::is_magnetic_flux_unit_v<inch_t>));
+ EXPECT_TRUE((traits::is_magnetic_flux_unit_v<maxwell_t, weber_t>));
+ EXPECT_FALSE((traits::is_magnetic_flux_unit_v<inch_t, weber_t>));
}
TEST_F(TypeTraits, is_magnetic_field_strength_unit) {
- EXPECT_TRUE((traits::is_magnetic_field_strength_unit<
- units::magnetic_field_strength::tesla>::value));
- EXPECT_TRUE((traits::is_magnetic_field_strength_unit<gauss>::value));
- EXPECT_FALSE((traits::is_magnetic_field_strength_unit<volt>::value));
- EXPECT_FALSE((traits::is_magnetic_field_strength_unit<double>::value));
+ EXPECT_TRUE((traits::is_magnetic_field_strength_unit_v<
+ units::magnetic_field_strength::tesla>));
+ EXPECT_TRUE((traits::is_magnetic_field_strength_unit_v<gauss>));
+ EXPECT_FALSE((traits::is_magnetic_field_strength_unit_v<volt>));
+ EXPECT_FALSE((traits::is_magnetic_field_strength_unit_v<double>));
- EXPECT_TRUE((traits::is_magnetic_field_strength_unit<tesla_t>::value));
- EXPECT_TRUE((traits::is_magnetic_field_strength_unit<const tesla_t>::value));
- EXPECT_TRUE((traits::is_magnetic_field_strength_unit<const tesla_t&>::value));
- EXPECT_TRUE((traits::is_magnetic_field_strength_unit<gauss_t>::value));
- EXPECT_FALSE((traits::is_magnetic_field_strength_unit<volt_t>::value));
+ EXPECT_TRUE((traits::is_magnetic_field_strength_unit_v<tesla_t>));
+ EXPECT_TRUE((traits::is_magnetic_field_strength_unit_v<const tesla_t>));
+ EXPECT_TRUE((traits::is_magnetic_field_strength_unit_v<const tesla_t&>));
+ EXPECT_TRUE((traits::is_magnetic_field_strength_unit_v<gauss_t>));
+ EXPECT_FALSE((traits::is_magnetic_field_strength_unit_v<volt_t>));
EXPECT_TRUE(
- (traits::is_magnetic_field_strength_unit<gauss_t, tesla_t>::value));
+ (traits::is_magnetic_field_strength_unit_v<gauss_t, tesla_t>));
EXPECT_FALSE(
- (traits::is_magnetic_field_strength_unit<volt_t, tesla_t>::value));
+ (traits::is_magnetic_field_strength_unit_v<volt_t, tesla_t>));
}
TEST_F(TypeTraits, is_inductance_unit) {
- EXPECT_TRUE((traits::is_inductance_unit<henry>::value));
- EXPECT_FALSE((traits::is_inductance_unit<farad>::value));
- EXPECT_FALSE((traits::is_inductance_unit<double>::value));
+ EXPECT_TRUE((traits::is_inductance_unit_v<henry>));
+ EXPECT_FALSE((traits::is_inductance_unit_v<farad>));
+ EXPECT_FALSE((traits::is_inductance_unit_v<double>));
- EXPECT_TRUE((traits::is_inductance_unit<henry_t>::value));
- EXPECT_TRUE((traits::is_inductance_unit<const henry_t>::value));
- EXPECT_TRUE((traits::is_inductance_unit<const henry_t&>::value));
- EXPECT_FALSE((traits::is_inductance_unit<farad_t>::value));
+ EXPECT_TRUE((traits::is_inductance_unit_v<henry_t>));
+ EXPECT_TRUE((traits::is_inductance_unit_v<const henry_t>));
+ EXPECT_TRUE((traits::is_inductance_unit_v<const henry_t&>));
+ EXPECT_FALSE((traits::is_inductance_unit_v<farad_t>));
EXPECT_TRUE(
- (traits::is_inductance_unit<const henry_t&, millihenry_t>::value));
- EXPECT_FALSE((traits::is_inductance_unit<farad_t, henry_t>::value));
+ (traits::is_inductance_unit_v<const henry_t&, millihenry_t>));
+ EXPECT_FALSE((traits::is_inductance_unit_v<farad_t, henry_t>));
}
TEST_F(TypeTraits, is_luminous_flux_unit) {
- EXPECT_TRUE((traits::is_luminous_flux_unit<lumen>::value));
- EXPECT_FALSE((traits::is_luminous_flux_unit<pound>::value));
- EXPECT_FALSE((traits::is_luminous_flux_unit<double>::value));
+ EXPECT_TRUE((traits::is_luminous_flux_unit_v<lumen>));
+ EXPECT_FALSE((traits::is_luminous_flux_unit_v<pound>));
+ EXPECT_FALSE((traits::is_luminous_flux_unit_v<double>));
- EXPECT_TRUE((traits::is_luminous_flux_unit<lumen_t>::value));
- EXPECT_TRUE((traits::is_luminous_flux_unit<const lumen_t>::value));
- EXPECT_TRUE((traits::is_luminous_flux_unit<const lumen_t&>::value));
- EXPECT_FALSE((traits::is_luminous_flux_unit<pound_t>::value));
+ EXPECT_TRUE((traits::is_luminous_flux_unit_v<lumen_t>));
+ EXPECT_TRUE((traits::is_luminous_flux_unit_v<const lumen_t>));
+ EXPECT_TRUE((traits::is_luminous_flux_unit_v<const lumen_t&>));
+ EXPECT_FALSE((traits::is_luminous_flux_unit_v<pound_t>));
EXPECT_TRUE(
- (traits::is_luminous_flux_unit<const lumen_t&, millilumen_t>::value));
- EXPECT_FALSE((traits::is_luminous_flux_unit<pound_t, lumen_t>::value));
+ (traits::is_luminous_flux_unit_v<const lumen_t&, millilumen_t>));
+ EXPECT_FALSE((traits::is_luminous_flux_unit_v<pound_t, lumen_t>));
}
TEST_F(TypeTraits, is_illuminance_unit) {
- EXPECT_TRUE((traits::is_illuminance_unit<illuminance::footcandle>::value));
- EXPECT_TRUE((traits::is_illuminance_unit<illuminance::lux>::value));
- EXPECT_FALSE((traits::is_illuminance_unit<meter>::value));
- EXPECT_FALSE((traits::is_illuminance_unit<double>::value));
+ EXPECT_TRUE((traits::is_illuminance_unit_v<illuminance::footcandle>));
+ EXPECT_TRUE((traits::is_illuminance_unit_v<illuminance::lux>));
+ EXPECT_FALSE((traits::is_illuminance_unit_v<meter>));
+ EXPECT_FALSE((traits::is_illuminance_unit_v<double>));
- EXPECT_TRUE((traits::is_illuminance_unit<footcandle_t>::value));
- EXPECT_TRUE((traits::is_illuminance_unit<const footcandle_t>::value));
- EXPECT_TRUE((traits::is_illuminance_unit<const footcandle_t&>::value));
- EXPECT_TRUE((traits::is_illuminance_unit<lux_t>::value));
- EXPECT_FALSE((traits::is_illuminance_unit<meter_t>::value));
- EXPECT_TRUE((traits::is_illuminance_unit<lux_t, footcandle_t>::value));
- EXPECT_FALSE((traits::is_illuminance_unit<meter_t, footcandle_t>::value));
+ EXPECT_TRUE((traits::is_illuminance_unit_v<footcandle_t>));
+ EXPECT_TRUE((traits::is_illuminance_unit_v<const footcandle_t>));
+ EXPECT_TRUE((traits::is_illuminance_unit_v<const footcandle_t&>));
+ EXPECT_TRUE((traits::is_illuminance_unit_v<lux_t>));
+ EXPECT_FALSE((traits::is_illuminance_unit_v<meter_t>));
+ EXPECT_TRUE((traits::is_illuminance_unit_v<lux_t, footcandle_t>));
+ EXPECT_FALSE((traits::is_illuminance_unit_v<meter_t, footcandle_t>));
}
TEST_F(TypeTraits, is_radioactivity_unit) {
- EXPECT_TRUE((traits::is_radioactivity_unit<becquerel>::value));
- EXPECT_FALSE((traits::is_radioactivity_unit<year>::value));
- EXPECT_FALSE((traits::is_radioactivity_unit<double>::value));
+ EXPECT_TRUE((traits::is_radioactivity_unit_v<becquerel>));
+ EXPECT_FALSE((traits::is_radioactivity_unit_v<year>));
+ EXPECT_FALSE((traits::is_radioactivity_unit_v<double>));
- EXPECT_TRUE((traits::is_radioactivity_unit<becquerel_t>::value));
- EXPECT_TRUE((traits::is_radioactivity_unit<const becquerel_t>::value));
- EXPECT_TRUE((traits::is_radioactivity_unit<const becquerel_t&>::value));
- EXPECT_FALSE((traits::is_radioactivity_unit<year_t>::value));
- EXPECT_TRUE((traits::is_radioactivity_unit<const becquerel_t&,
- millibecquerel_t>::value));
- EXPECT_FALSE((traits::is_radioactivity_unit<year_t, becquerel_t>::value));
+ EXPECT_TRUE((traits::is_radioactivity_unit_v<becquerel_t>));
+ EXPECT_TRUE((traits::is_radioactivity_unit_v<const becquerel_t>));
+ EXPECT_TRUE((traits::is_radioactivity_unit_v<const becquerel_t&>));
+ EXPECT_FALSE((traits::is_radioactivity_unit_v<year_t>));
+ EXPECT_TRUE((traits::is_radioactivity_unit_v<const becquerel_t&,
+ millibecquerel_t>));
+ EXPECT_FALSE((traits::is_radioactivity_unit_v<year_t, becquerel_t>));
}
TEST_F(TypeTraits, is_torque_unit) {
- EXPECT_TRUE((traits::is_torque_unit<torque::newton_meter>::value));
- EXPECT_TRUE((traits::is_torque_unit<torque::foot_pound>::value));
- EXPECT_FALSE((traits::is_torque_unit<volume::cubic_meter>::value));
- EXPECT_FALSE((traits::is_torque_unit<double>::value));
+ EXPECT_TRUE((traits::is_torque_unit_v<torque::newton_meter>));
+ EXPECT_TRUE((traits::is_torque_unit_v<torque::foot_pound>));
+ EXPECT_FALSE((traits::is_torque_unit_v<volume::cubic_meter>));
+ EXPECT_FALSE((traits::is_torque_unit_v<double>));
- EXPECT_TRUE((traits::is_torque_unit<torque::newton_meter_t>::value));
- EXPECT_TRUE((traits::is_torque_unit<const torque::newton_meter_t>::value));
- EXPECT_TRUE((traits::is_torque_unit<const torque::newton_meter_t&>::value));
- EXPECT_TRUE((traits::is_torque_unit<torque::foot_pound_t>::value));
- EXPECT_FALSE((traits::is_torque_unit<volume::cubic_meter_t>::value));
- EXPECT_TRUE((traits::is_torque_unit<torque::foot_pound_t,
- torque::newton_meter_t>::value));
- EXPECT_FALSE((traits::is_torque_unit<volume::cubic_meter_t,
- torque::newton_meter_t>::value));
+ EXPECT_TRUE((traits::is_torque_unit_v<torque::newton_meter_t>));
+ EXPECT_TRUE((traits::is_torque_unit_v<const torque::newton_meter_t>));
+ EXPECT_TRUE((traits::is_torque_unit_v<const torque::newton_meter_t&>));
+ EXPECT_TRUE((traits::is_torque_unit_v<torque::foot_pound_t>));
+ EXPECT_FALSE((traits::is_torque_unit_v<volume::cubic_meter_t>));
+ EXPECT_TRUE((traits::is_torque_unit_v<torque::foot_pound_t,
+ torque::newton_meter_t>));
+ EXPECT_FALSE((traits::is_torque_unit_v<volume::cubic_meter_t,
+ torque::newton_meter_t>));
}
TEST_F(TypeTraits, is_area_unit) {
- EXPECT_TRUE((traits::is_area_unit<square_meter>::value));
- EXPECT_TRUE((traits::is_area_unit<hectare>::value));
- EXPECT_FALSE((traits::is_area_unit<astronicalUnit>::value));
- EXPECT_FALSE((traits::is_area_unit<double>::value));
+ EXPECT_TRUE((traits::is_area_unit_v<square_meter>));
+ EXPECT_TRUE((traits::is_area_unit_v<hectare>));
+ EXPECT_FALSE((traits::is_area_unit_v<astronicalUnit>));
+ EXPECT_FALSE((traits::is_area_unit_v<double>));
- EXPECT_TRUE((traits::is_area_unit<square_meter_t>::value));
- EXPECT_TRUE((traits::is_area_unit<const square_meter_t>::value));
- EXPECT_TRUE((traits::is_area_unit<const square_meter_t&>::value));
- EXPECT_TRUE((traits::is_area_unit<hectare_t>::value));
- EXPECT_FALSE((traits::is_area_unit<astronicalUnit_t>::value));
- EXPECT_TRUE((traits::is_area_unit<hectare_t, square_meter_t>::value));
- EXPECT_FALSE((traits::is_area_unit<astronicalUnit_t, square_meter_t>::value));
+ EXPECT_TRUE((traits::is_area_unit_v<square_meter_t>));
+ EXPECT_TRUE((traits::is_area_unit_v<const square_meter_t>));
+ EXPECT_TRUE((traits::is_area_unit_v<const square_meter_t&>));
+ EXPECT_TRUE((traits::is_area_unit_v<hectare_t>));
+ EXPECT_FALSE((traits::is_area_unit_v<astronicalUnit_t>));
+ EXPECT_TRUE((traits::is_area_unit_v<hectare_t, square_meter_t>));
+ EXPECT_FALSE((traits::is_area_unit_v<astronicalUnit_t, square_meter_t>));
}
TEST_F(TypeTraits, is_volume_unit) {
- EXPECT_TRUE((traits::is_volume_unit<cubic_meter>::value));
- EXPECT_TRUE((traits::is_volume_unit<cubic_foot>::value));
- EXPECT_FALSE((traits::is_volume_unit<square_feet>::value));
- EXPECT_FALSE((traits::is_volume_unit<double>::value));
+ EXPECT_TRUE((traits::is_volume_unit_v<cubic_meter>));
+ EXPECT_TRUE((traits::is_volume_unit_v<cubic_foot>));
+ EXPECT_FALSE((traits::is_volume_unit_v<square_feet>));
+ EXPECT_FALSE((traits::is_volume_unit_v<double>));
- EXPECT_TRUE((traits::is_volume_unit<cubic_meter_t>::value));
- EXPECT_TRUE((traits::is_volume_unit<const cubic_meter_t>::value));
- EXPECT_TRUE((traits::is_volume_unit<const cubic_meter_t&>::value));
- EXPECT_TRUE((traits::is_volume_unit<cubic_inch_t>::value));
- EXPECT_FALSE((traits::is_volume_unit<foot_t>::value));
- EXPECT_TRUE((traits::is_volume_unit<cubic_inch_t, cubic_meter_t>::value));
- EXPECT_FALSE((traits::is_volume_unit<foot_t, cubic_meter_t>::value));
+ EXPECT_TRUE((traits::is_volume_unit_v<cubic_meter_t>));
+ EXPECT_TRUE((traits::is_volume_unit_v<const cubic_meter_t>));
+ EXPECT_TRUE((traits::is_volume_unit_v<const cubic_meter_t&>));
+ EXPECT_TRUE((traits::is_volume_unit_v<cubic_inch_t>));
+ EXPECT_FALSE((traits::is_volume_unit_v<foot_t>));
+ EXPECT_TRUE((traits::is_volume_unit_v<cubic_inch_t, cubic_meter_t>));
+ EXPECT_FALSE((traits::is_volume_unit_v<foot_t, cubic_meter_t>));
}
TEST_F(TypeTraits, is_density_unit) {
- EXPECT_TRUE((traits::is_density_unit<kilograms_per_cubic_meter>::value));
- EXPECT_TRUE((traits::is_density_unit<ounces_per_cubic_foot>::value));
- EXPECT_FALSE((traits::is_density_unit<year>::value));
- EXPECT_FALSE((traits::is_density_unit<double>::value));
+ EXPECT_TRUE((traits::is_density_unit_v<kilograms_per_cubic_meter>));
+ EXPECT_TRUE((traits::is_density_unit_v<ounces_per_cubic_foot>));
+ EXPECT_FALSE((traits::is_density_unit_v<year>));
+ EXPECT_FALSE((traits::is_density_unit_v<double>));
- EXPECT_TRUE((traits::is_density_unit<kilograms_per_cubic_meter_t>::value));
+ EXPECT_TRUE((traits::is_density_unit_v<kilograms_per_cubic_meter_t>));
EXPECT_TRUE(
- (traits::is_density_unit<const kilograms_per_cubic_meter_t>::value));
+ (traits::is_density_unit_v<const kilograms_per_cubic_meter_t>));
EXPECT_TRUE(
- (traits::is_density_unit<const kilograms_per_cubic_meter_t&>::value));
- EXPECT_TRUE((traits::is_density_unit<ounces_per_cubic_foot_t>::value));
- EXPECT_FALSE((traits::is_density_unit<year_t>::value));
- EXPECT_TRUE((traits::is_density_unit<ounces_per_cubic_foot_t,
- kilograms_per_cubic_meter_t>::value));
+ (traits::is_density_unit_v<const kilograms_per_cubic_meter_t&>));
+ EXPECT_TRUE((traits::is_density_unit_v<ounces_per_cubic_foot_t>));
+ EXPECT_FALSE((traits::is_density_unit_v<year_t>));
+ EXPECT_TRUE((traits::is_density_unit_v<ounces_per_cubic_foot_t,
+ kilograms_per_cubic_meter_t>));
EXPECT_FALSE(
- (traits::is_density_unit<year_t, kilograms_per_cubic_meter_t>::value));
+ (traits::is_density_unit_v<year_t, kilograms_per_cubic_meter_t>));
}
TEST_F(TypeTraits, is_data_unit) {
- EXPECT_TRUE((traits::is_data_unit<bit>::value));
- EXPECT_TRUE((traits::is_data_unit<byte>::value));
- EXPECT_TRUE((traits::is_data_unit<exabit>::value));
- EXPECT_TRUE((traits::is_data_unit<exabyte>::value));
- EXPECT_FALSE((traits::is_data_unit<year>::value));
- EXPECT_FALSE((traits::is_data_unit<double>::value));
+ EXPECT_TRUE((traits::is_data_unit_v<bit>));
+ EXPECT_TRUE((traits::is_data_unit_v<byte>));
+ EXPECT_TRUE((traits::is_data_unit_v<exabit>));
+ EXPECT_TRUE((traits::is_data_unit_v<exabyte>));
+ EXPECT_FALSE((traits::is_data_unit_v<year>));
+ EXPECT_FALSE((traits::is_data_unit_v<double>));
- EXPECT_TRUE((traits::is_data_unit<bit_t>::value));
- EXPECT_TRUE((traits::is_data_unit<const bit_t>::value));
- EXPECT_TRUE((traits::is_data_unit<const bit_t&>::value));
- EXPECT_TRUE((traits::is_data_unit<byte_t>::value));
- EXPECT_FALSE((traits::is_data_unit<year_t>::value));
- EXPECT_TRUE((traits::is_data_unit<bit_t, byte_t>::value));
- EXPECT_FALSE((traits::is_data_unit<year_t, byte_t>::value));
+ EXPECT_TRUE((traits::is_data_unit_v<bit_t>));
+ EXPECT_TRUE((traits::is_data_unit_v<const bit_t>));
+ EXPECT_TRUE((traits::is_data_unit_v<const bit_t&>));
+ EXPECT_TRUE((traits::is_data_unit_v<byte_t>));
+ EXPECT_FALSE((traits::is_data_unit_v<year_t>));
+ EXPECT_TRUE((traits::is_data_unit_v<bit_t, byte_t>));
+ EXPECT_FALSE((traits::is_data_unit_v<year_t, byte_t>));
}
TEST_F(TypeTraits, is_data_transfer_rate_unit) {
- EXPECT_TRUE((traits::is_data_transfer_rate_unit<Gbps>::value));
- EXPECT_TRUE((traits::is_data_transfer_rate_unit<GBps>::value));
- EXPECT_FALSE((traits::is_data_transfer_rate_unit<year>::value));
- EXPECT_FALSE((traits::is_data_transfer_rate_unit<double>::value));
+ EXPECT_TRUE((traits::is_data_transfer_rate_unit_v<Gbps>));
+ EXPECT_TRUE((traits::is_data_transfer_rate_unit_v<GBps>));
+ EXPECT_FALSE((traits::is_data_transfer_rate_unit_v<year>));
+ EXPECT_FALSE((traits::is_data_transfer_rate_unit_v<double>));
EXPECT_TRUE(
- (traits::is_data_transfer_rate_unit<gigabits_per_second_t>::value));
+ (traits::is_data_transfer_rate_unit_v<gigabits_per_second_t>));
EXPECT_TRUE((
- traits::is_data_transfer_rate_unit<const gigabytes_per_second_t>::value));
- EXPECT_TRUE((traits::is_data_transfer_rate_unit<
- const gigabytes_per_second_t&>::value));
+ traits::is_data_transfer_rate_unit_v<const gigabytes_per_second_t>));
+ EXPECT_TRUE((traits::is_data_transfer_rate_unit_v<
+ const gigabytes_per_second_t&>));
EXPECT_TRUE(
- (traits::is_data_transfer_rate_unit<gigabytes_per_second_t>::value));
- EXPECT_FALSE((traits::is_data_transfer_rate_unit<year_t>::value));
+ (traits::is_data_transfer_rate_unit_v<gigabytes_per_second_t>));
+ EXPECT_FALSE((traits::is_data_transfer_rate_unit_v<year_t>));
EXPECT_TRUE(
- (traits::is_data_transfer_rate_unit<gigabits_per_second_t,
- gigabytes_per_second_t>::value));
+ (traits::is_data_transfer_rate_unit_v<gigabits_per_second_t,
+ gigabytes_per_second_t>));
EXPECT_FALSE(
- (traits::is_data_transfer_rate_unit<year_t,
- gigabytes_per_second_t>::value));
+ (traits::is_data_transfer_rate_unit_v<year_t,
+ gigabytes_per_second_t>));
}
TEST_F(UnitManipulators, squared) {
@@ -861,8 +861,8 @@
using scalar_2 = squared<scalar>; // this is actually nonsensical, and should
// also result in a scalar.
bool isSame =
- std::is_same<typename std::decay<scalar_t>::type,
- typename std::decay<unit_t<scalar_2>>::type>::value;
+ std::is_same_v<typename std::decay_t<scalar_t>,
+ typename std::decay_t<unit_t<scalar_2>>>;
EXPECT_TRUE(isSame);
}
@@ -877,9 +877,9 @@
double test;
test = convert<square_root<square_kilometer>, meter>(1.0);
- EXPECT_TRUE((traits::is_convertible_unit<
- typename std::decay<square_root<square_kilometer>>::type,
- kilometer>::value));
+ EXPECT_TRUE((traits::is_convertible_unit_v<
+ typename std::decay_t<square_root<square_kilometer>>,
+ kilometer>));
EXPECT_NEAR(1000.0, test, 5.0e-13);
}
@@ -893,10 +893,10 @@
using acceleration4 = compound_unit<meters, inverse<squared<seconds>>>;
using acceleration5 = compound_unit<meters, squared<inverse<seconds>>>;
- bool areSame12 = std::is_same<acceleration1, acceleration2>::value;
- bool areSame23 = std::is_same<acceleration2, acceleration3>::value;
- bool areSame34 = std::is_same<acceleration3, acceleration4>::value;
- bool areSame45 = std::is_same<acceleration4, acceleration5>::value;
+ bool areSame12 = std::is_same_v<acceleration1, acceleration2>;
+ bool areSame23 = std::is_same_v<acceleration2, acceleration3>;
+ bool areSame34 = std::is_same_v<acceleration3, acceleration4>;
+ bool areSame45 = std::is_same_v<acceleration4, acceleration5>;
EXPECT_TRUE(areSame12);
EXPECT_TRUE(areSame23);
@@ -907,7 +907,7 @@
using arbitrary1 = compound_unit<meters, inverse<celsius>>;
using arbitrary2 = compound_unit<meters, celsius>;
using arbitrary3 = compound_unit<arbitrary1, arbitrary2>;
- EXPECT_TRUE((std::is_same<square_meters, arbitrary3>::value));
+ EXPECT_TRUE((std::is_same_v<square_meters, arbitrary3>));
}
TEST_F(UnitManipulators, dimensionalAnalysis) {
@@ -916,47 +916,47 @@
// template parameters), as you can get the resulting unit of the operation.
using velocity = units::detail::unit_divide<meters, second>;
- bool shouldBeTrue = std::is_same<meters_per_second, velocity>::value;
+ bool shouldBeTrue = std::is_same_v<meters_per_second, velocity>;
EXPECT_TRUE(shouldBeTrue);
using acceleration1 = unit<std::ratio<1>, category::acceleration_unit>;
using acceleration2 = units::detail::unit_divide<
meters, units::detail::unit_multiply<seconds, seconds>>;
- shouldBeTrue = std::is_same<acceleration1, acceleration2>::value;
+ shouldBeTrue = std::is_same_v<acceleration1, acceleration2>;
EXPECT_TRUE(shouldBeTrue);
}
#ifdef _MSC_VER
#if (_MSC_VER >= 1900)
TEST_F(UnitContainer, trivial) {
- EXPECT_TRUE((std::is_trivial<meter_t>::value));
- EXPECT_TRUE((std::is_trivially_assignable<meter_t, meter_t>::value));
- EXPECT_TRUE((std::is_trivially_constructible<meter_t>::value));
- EXPECT_TRUE((std::is_trivially_copy_assignable<meter_t>::value));
- EXPECT_TRUE((std::is_trivially_copy_constructible<meter_t>::value));
- EXPECT_TRUE((std::is_trivially_copyable<meter_t>::value));
- EXPECT_TRUE((std::is_trivially_default_constructible<meter_t>::value));
- EXPECT_TRUE((std::is_trivially_destructible<meter_t>::value));
- EXPECT_TRUE((std::is_trivially_move_assignable<meter_t>::value));
- EXPECT_TRUE((std::is_trivially_move_constructible<meter_t>::value));
+ EXPECT_TRUE((std::is_trivial_v<meter_t>));
+ EXPECT_TRUE((std::is_trivially_assignable_v<meter_t, meter_t>));
+ EXPECT_TRUE((std::is_trivially_constructible_v<meter_t>));
+ EXPECT_TRUE((std::is_trivially_copy_assignable_v<meter_t>));
+ EXPECT_TRUE((std::is_trivially_copy_constructible_v<meter_t>));
+ EXPECT_TRUE((std::is_trivially_copyable_v<meter_t>));
+ EXPECT_TRUE((std::is_trivially_default_constructible_v<meter_t>));
+ EXPECT_TRUE((std::is_trivially_destructible_v<meter_t>));
+ EXPECT_TRUE((std::is_trivially_move_assignable_v<meter_t>));
+ EXPECT_TRUE((std::is_trivially_move_constructible_v<meter_t>));
- EXPECT_TRUE((std::is_trivial<dB_t>::value));
- EXPECT_TRUE((std::is_trivially_assignable<dB_t, dB_t>::value));
- EXPECT_TRUE((std::is_trivially_constructible<dB_t>::value));
- EXPECT_TRUE((std::is_trivially_copy_assignable<dB_t>::value));
- EXPECT_TRUE((std::is_trivially_copy_constructible<dB_t>::value));
- EXPECT_TRUE((std::is_trivially_copyable<dB_t>::value));
- EXPECT_TRUE((std::is_trivially_default_constructible<dB_t>::value));
- EXPECT_TRUE((std::is_trivially_destructible<dB_t>::value));
- EXPECT_TRUE((std::is_trivially_move_assignable<dB_t>::value));
- EXPECT_TRUE((std::is_trivially_move_constructible<dB_t>::value));
+ EXPECT_TRUE((std::is_trivial_v<dB_t>));
+ EXPECT_TRUE((std::is_trivially_assignable_v<dB_t, dB_t>));
+ EXPECT_TRUE((std::is_trivially_constructible_v<dB_t>));
+ EXPECT_TRUE((std::is_trivially_copy_assignable_v<dB_t>));
+ EXPECT_TRUE((std::is_trivially_copy_constructible_v<dB_t>));
+ EXPECT_TRUE((std::is_trivially_copyable_v<dB_t>));
+ EXPECT_TRUE((std::is_trivially_default_constructible_v<dB_t>));
+ EXPECT_TRUE((std::is_trivially_destructible_v<dB_t>));
+ EXPECT_TRUE((std::is_trivially_move_assignable_v<dB_t>));
+ EXPECT_TRUE((std::is_trivially_move_constructible_v<dB_t>));
}
#endif
#endif
TEST_F(UnitContainer, has_value_member) {
- EXPECT_TRUE((traits::has_value_member<linear_scale<double>, double>::value));
- EXPECT_FALSE((traits::has_value_member<meter, double>::value));
+ EXPECT_TRUE((traits::has_value_member_v<linear_scale<double>, double>));
+ EXPECT_FALSE((traits::has_value_member_v<meter, double>));
}
TEST_F(UnitContainer, make_unit) {
@@ -1162,7 +1162,7 @@
result_m = a_m * 4.0;
EXPECT_NEAR(4.0, result_m(), 5.0e-5);
- bool isSame = std::is_same<decltype(result_m), meter_t>::value;
+ bool isSame = std::is_same_v<decltype(result_m), meter_t>;
EXPECT_TRUE(isSame);
}
@@ -1174,27 +1174,27 @@
auto c = a_m / a_ft;
EXPECT_NEAR(1.0, c, 5.0e-5);
- isSame = std::is_same<decltype(c), scalar_t>::value;
+ isSame = std::is_same_v<decltype(c), scalar_t>;
EXPECT_TRUE(isSame);
c = a_m / b_m;
EXPECT_NEAR(0.5, c, 5.0e-5);
- isSame = std::is_same<decltype(c), scalar_t>::value;
+ isSame = std::is_same_v<decltype(c), scalar_t>;
EXPECT_TRUE(isSame);
c = a_ft / a_m;
EXPECT_NEAR(1.0, c, 5.0e-5);
- isSame = std::is_same<decltype(c), scalar_t>::value;
+ isSame = std::is_same_v<decltype(c), scalar_t>;
EXPECT_TRUE(isSame);
c = scalar_t(1.0) / 2.0;
EXPECT_NEAR(0.5, c, 5.0e-5);
- isSame = std::is_same<decltype(c), scalar_t>::value;
+ isSame = std::is_same_v<decltype(c), scalar_t>;
EXPECT_TRUE(isSame);
c = 1.0 / scalar_t(2.0);
EXPECT_NEAR(0.5, c, 5.0e-5);
- isSame = std::is_same<decltype(c), scalar_t>::value;
+ isSame = std::is_same_v<decltype(c), scalar_t>;
EXPECT_TRUE(isSame);
double d = scalar_t(1.0) / 2.0;
@@ -1202,17 +1202,17 @@
auto e = a_m / a_sec;
EXPECT_NEAR(0.1, e(), 5.0e-5);
- isSame = std::is_same<decltype(e), meters_per_second_t>::value;
+ isSame = std::is_same_v<decltype(e), meters_per_second_t>;
EXPECT_TRUE(isSame);
auto f = a_m / 8.0;
EXPECT_NEAR(0.125, f(), 5.0e-5);
- isSame = std::is_same<decltype(f), meter_t>::value;
+ isSame = std::is_same_v<decltype(f), meter_t>;
EXPECT_TRUE(isSame);
auto g = 4.0 / b_m;
EXPECT_NEAR(2.0, g(), 5.0e-5);
- isSame = std::is_same<decltype(g), unit_t<inverse<meters>>>::value;
+ isSame = std::is_same_v<decltype(g), unit_t<inverse<meters>>>;
EXPECT_TRUE(isSame);
auto mph = mile_t(60.0) / hour_t(1.0);
@@ -1329,7 +1329,7 @@
auto test2 = meter_t(4.0).value();
EXPECT_DOUBLE_EQ(4.0, test2);
- EXPECT_TRUE((std::is_same<decltype(test2), double>::value));
+ EXPECT_TRUE((std::is_same_v<decltype(test2), double>));
}
TEST_F(UnitContainer, convertMethod) {
@@ -1613,7 +1613,7 @@
EXPECT_NEAR(40.0, result_dbw(), 5.0e-5);
result_dbw = dB_t(12.0) + dBW_t(30.0);
EXPECT_NEAR(42.0, result_dbw(), 5.0e-5);
- isSame = std::is_same<decltype(result_dbw), dBW_t>::value;
+ isSame = std::is_same_v<decltype(result_dbw), dBW_t>;
EXPECT_TRUE(isSame);
auto result_dbm = dB_t(30.0) + dBm_t(20.0);
@@ -1623,8 +1623,8 @@
// it works...
auto result_dBW2 = dBW_t(10.0) + dBm_t(40.0);
EXPECT_NEAR(20.0, result_dBW2(), 5.0e-5);
- isSame = std::is_same<decltype(result_dBW2),
- unit_t<squared<watts>, double, decibel_scale>>::value;
+ isSame = std::is_same_v<decltype(result_dBW2),
+ unit_t<squared<watts>, double, decibel_scale>>;
EXPECT_TRUE(isSame);
}
@@ -1633,22 +1633,22 @@
auto result_dbw = dBW_t(10.0) - dB_t(30.0);
EXPECT_NEAR(-20.0, result_dbw(), 5.0e-5);
- isSame = std::is_same<decltype(result_dbw), dBW_t>::value;
+ isSame = std::is_same_v<decltype(result_dbw), dBW_t>;
EXPECT_TRUE(isSame);
auto result_dbm = dBm_t(100.0) - dB_t(30.0);
EXPECT_NEAR(70.0, result_dbm(), 5.0e-5);
- isSame = std::is_same<decltype(result_dbm), dBm_t>::value;
+ isSame = std::is_same_v<decltype(result_dbm), dBm_t>;
EXPECT_TRUE(isSame);
auto result_db = dBW_t(100.0) - dBW_t(80.0);
EXPECT_NEAR(20.0, result_db(), 5.0e-5);
- isSame = std::is_same<decltype(result_db), dB_t>::value;
+ isSame = std::is_same_v<decltype(result_db), dB_t>;
EXPECT_TRUE(isSame);
result_db = dB_t(100.0) - dB_t(80.0);
EXPECT_NEAR(20.0, result_db(), 5.0e-5);
- isSame = std::is_same<decltype(result_db), dB_t>::value;
+ isSame = std::is_same_v<decltype(result_db), dB_t>;
EXPECT_TRUE(isSame);
}
@@ -1666,25 +1666,25 @@
EXPECT_EQ(iResult2, unit_cast<int>(test2));
EXPECT_TRUE(
- (std::is_same<double, decltype(unit_cast<double>(test1))>::value));
- EXPECT_TRUE((std::is_same<int, decltype(unit_cast<int>(test2))>::value));
+ (std::is_same_v<double, decltype(unit_cast<double>(test1))>));
+ EXPECT_TRUE((std::is_same_v<int, decltype(unit_cast<int>(test2))>));
}
// literal syntax is only supported in GCC 4.7+ and MSVC2015+
#if !defined(_MSC_VER) || _MSC_VER > 1800
TEST_F(UnitContainer, literals) {
// basic functionality testing
- EXPECT_TRUE((std::is_same<decltype(16.2_m), meter_t>::value));
+ EXPECT_TRUE((std::is_same_v<decltype(16.2_m), meter_t>));
EXPECT_TRUE(meter_t(16.2) == 16.2_m);
EXPECT_TRUE(meter_t(16) == 16_m);
- EXPECT_TRUE((std::is_same<decltype(11.2_ft), foot_t>::value));
+ EXPECT_TRUE((std::is_same_v<decltype(11.2_ft), foot_t>));
EXPECT_TRUE(foot_t(11.2) == 11.2_ft);
EXPECT_TRUE(foot_t(11) == 11_ft);
// auto using literal syntax
auto x = 10.0_m;
- EXPECT_TRUE((std::is_same<decltype(x), meter_t>::value));
+ EXPECT_TRUE((std::is_same_v<decltype(x), meter_t>));
EXPECT_TRUE(meter_t(10) == x);
// conversion using literal syntax
@@ -1920,8 +1920,8 @@
double test;
bool same;
- same = std::is_same<traits::base_unit_of<steradians>,
- traits::base_unit_of<degrees_squared>>::value;
+ same = std::is_same_v<traits::base_unit_of<steradians>,
+ traits::base_unit_of<degrees_squared>>;
EXPECT_TRUE(same);
test = convert<steradians, steradians>(72.0);
@@ -1961,10 +1961,10 @@
double test;
bool same;
- same = std::is_same<meters_per_second,
- unit<std::ratio<1>, category::velocity_unit>>::value;
+ same = std::is_same_v<meters_per_second,
+ unit<std::ratio<1>, category::velocity_unit>>;
EXPECT_TRUE(same);
- same = traits::is_convertible_unit<miles_per_hour, meters_per_second>::value;
+ same = traits::is_convertible_unit_v<miles_per_hour, meters_per_second>;
EXPECT_TRUE(same);
test = convert<meters_per_second, miles_per_hour>(1250.0);
@@ -1984,10 +1984,10 @@
bool same;
same =
- std::is_same<radians_per_second,
- unit<std::ratio<1>, category::angular_velocity_unit>>::value;
+ std::is_same_v<radians_per_second,
+ unit<std::ratio<1>, category::angular_velocity_unit>>;
EXPECT_TRUE(same);
- same = traits::is_convertible_unit<rpm, radians_per_second>::value;
+ same = traits::is_convertible_unit_v<rpm, radians_per_second>;
EXPECT_TRUE(same);
test = convert<radians_per_second, milliarcseconds_per_year>(1.0);
@@ -2612,8 +2612,8 @@
TEST_F(UnitConversion, pi) {
EXPECT_TRUE(
- units::traits::is_dimensionless_unit<decltype(constants::pi)>::value);
- EXPECT_TRUE(units::traits::is_dimensionless_unit<constants::PI>::value);
+ units::traits::is_dimensionless_unit_v<decltype(constants::pi)>);
+ EXPECT_TRUE(units::traits::is_dimensionless_unit_v<constants::PI>);
// implicit conversion/arithmetic
EXPECT_NEAR(3.14159, constants::pi, 5.0e-6);
@@ -2635,9 +2635,9 @@
// auto multiplication
EXPECT_TRUE(
- (std::is_same<meter_t, decltype(constants::pi * meter_t(1))>::value));
+ (std::is_same_v<meter_t, decltype(constants::pi * meter_t(1))>));
EXPECT_TRUE(
- (std::is_same<meter_t, decltype(meter_t(1) * constants::pi)>::value));
+ (std::is_same_v<meter_t, decltype(meter_t(1) * constants::pi)>));
EXPECT_NEAR(constants::detail::PI_VAL,
(constants::pi * meter_t(1)).value(), 5.0e-10);
@@ -2653,9 +2653,9 @@
// auto division
EXPECT_TRUE(
- (std::is_same<hertz_t, decltype(constants::pi / second_t(1))>::value));
+ (std::is_same_v<hertz_t, decltype(constants::pi / second_t(1))>));
EXPECT_TRUE(
- (std::is_same<second_t, decltype(second_t(1) / constants::pi)>::value));
+ (std::is_same_v<second_t, decltype(second_t(1) / constants::pi)>));
EXPECT_NEAR(constants::detail::PI_VAL,
(constants::pi / second_t(1)).value(), 5.0e-10);
@@ -2745,35 +2745,35 @@
}
TEST_F(UnitMath, cos) {
- EXPECT_TRUE((std::is_same<typename std::decay<scalar_t>::type,
- typename std::decay<decltype(
- cos(angle::radian_t(0)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<scalar_t>,
+ typename std::decay_t<decltype(
+ cos(angle::radian_t(0)))>>));
EXPECT_NEAR(scalar_t(-0.41614683654), cos(angle::radian_t(2)), 5.0e-11);
EXPECT_NEAR(scalar_t(-0.70710678118), cos(angle::degree_t(135)),
5.0e-11);
}
TEST_F(UnitMath, sin) {
- EXPECT_TRUE((std::is_same<typename std::decay<scalar_t>::type,
- typename std::decay<decltype(
- sin(angle::radian_t(0)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<scalar_t>,
+ typename std::decay_t<decltype(
+ sin(angle::radian_t(0)))>>));
EXPECT_NEAR(scalar_t(0.90929742682), sin(angle::radian_t(2)), 5.0e-11);
EXPECT_NEAR(scalar_t(0.70710678118), sin(angle::degree_t(135)), 5.0e-11);
}
TEST_F(UnitMath, tan) {
- EXPECT_TRUE((std::is_same<typename std::decay<scalar_t>::type,
- typename std::decay<decltype(
- tan(angle::radian_t(0)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<scalar_t>,
+ typename std::decay_t<decltype(
+ tan(angle::radian_t(0)))>>));
EXPECT_NEAR(scalar_t(-2.18503986326), tan(angle::radian_t(2)), 5.0e-11);
EXPECT_NEAR(scalar_t(-1.0), tan(angle::degree_t(135)), 5.0e-11);
}
TEST_F(UnitMath, acos) {
EXPECT_TRUE(
- (std::is_same<
- typename std::decay<angle::radian_t>::type,
- typename std::decay<decltype(acos(scalar_t(0)))>::type>::value));
+ (std::is_same_v<
+ typename std::decay_t<angle::radian_t>,
+ typename std::decay_t<decltype(acos(scalar_t(0)))>>));
EXPECT_NEAR(angle::radian_t(2).value(),
acos(scalar_t(-0.41614683654)).value(), 5.0e-11);
EXPECT_NEAR(
@@ -2785,9 +2785,9 @@
TEST_F(UnitMath, asin) {
EXPECT_TRUE(
- (std::is_same<
- typename std::decay<angle::radian_t>::type,
- typename std::decay<decltype(asin(scalar_t(0)))>::type>::value));
+ (std::is_same_v<
+ typename std::decay_t<angle::radian_t>,
+ typename std::decay_t<decltype(asin(scalar_t(0)))>>));
EXPECT_NEAR(angle::radian_t(1.14159265).value(),
asin(scalar_t(0.90929742682)).value(), 5.0e-9);
EXPECT_NEAR(
@@ -2799,9 +2799,9 @@
TEST_F(UnitMath, atan) {
EXPECT_TRUE(
- (std::is_same<
- typename std::decay<angle::radian_t>::type,
- typename std::decay<decltype(atan(scalar_t(0)))>::type>::value));
+ (std::is_same_v<
+ typename std::decay_t<angle::radian_t>,
+ typename std::decay_t<decltype(atan(scalar_t(0)))>>));
EXPECT_NEAR(angle::radian_t(-1.14159265).value(),
atan(scalar_t(-2.18503986326)).value(), 5.0e-9);
EXPECT_NEAR(angle::degree_t(-45).value(),
@@ -2809,9 +2809,9 @@
}
TEST_F(UnitMath, atan2) {
- EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
- typename std::decay<decltype(atan2(
- scalar_t(1), scalar_t(1)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<angle::radian_t>,
+ typename std::decay_t<decltype(atan2(
+ scalar_t(1), scalar_t(1)))>>));
EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 4).value(),
atan2(scalar_t(2), scalar_t(2)).value(), 5.0e-12);
EXPECT_NEAR(
@@ -2819,9 +2819,9 @@
angle::degree_t(atan2(scalar_t(2), scalar_t(2))).value(),
5.0e-12);
- EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
- typename std::decay<decltype(atan2(
- scalar_t(1), scalar_t(1)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<angle::radian_t>,
+ typename std::decay_t<decltype(atan2(
+ scalar_t(1), scalar_t(1)))>>));
EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 6).value(),
atan2(scalar_t(1), scalar_t(std::sqrt(3))).value(),
5.0e-12);
@@ -2832,33 +2832,33 @@
}
TEST_F(UnitMath, cosh) {
- EXPECT_TRUE((std::is_same<typename std::decay<scalar_t>::type,
- typename std::decay<decltype(
- cosh(angle::radian_t(0)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<scalar_t>,
+ typename std::decay_t<decltype(
+ cosh(angle::radian_t(0)))>>));
EXPECT_NEAR(scalar_t(3.76219569108), cosh(angle::radian_t(2)), 5.0e-11);
EXPECT_NEAR(scalar_t(5.32275215), cosh(angle::degree_t(135)), 5.0e-9);
}
TEST_F(UnitMath, sinh) {
- EXPECT_TRUE((std::is_same<typename std::decay<scalar_t>::type,
- typename std::decay<decltype(
- sinh(angle::radian_t(0)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<scalar_t>,
+ typename std::decay_t<decltype(
+ sinh(angle::radian_t(0)))>>));
EXPECT_NEAR(scalar_t(3.62686040785), sinh(angle::radian_t(2)), 5.0e-11);
EXPECT_NEAR(scalar_t(5.22797192), sinh(angle::degree_t(135)), 5.0e-9);
}
TEST_F(UnitMath, tanh) {
- EXPECT_TRUE((std::is_same<typename std::decay<scalar_t>::type,
- typename std::decay<decltype(
- tanh(angle::radian_t(0)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<scalar_t>,
+ typename std::decay_t<decltype(
+ tanh(angle::radian_t(0)))>>));
EXPECT_NEAR(scalar_t(0.96402758007), tanh(angle::radian_t(2)), 5.0e-11);
EXPECT_NEAR(scalar_t(0.98219338), tanh(angle::degree_t(135)), 5.0e-11);
}
TEST_F(UnitMath, acosh) {
- EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
- typename std::decay<decltype(
- acosh(scalar_t(0)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<angle::radian_t>,
+ typename std::decay_t<decltype(
+ acosh(scalar_t(0)))>>));
EXPECT_NEAR(angle::radian_t(1.316957896924817).value(),
acosh(scalar_t(2.0)).value(), 5.0e-11);
EXPECT_NEAR(angle::degree_t(75.456129290216893).value(),
@@ -2866,9 +2866,9 @@
}
TEST_F(UnitMath, asinh) {
- EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
- typename std::decay<decltype(
- asinh(scalar_t(0)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<angle::radian_t>,
+ typename std::decay_t<decltype(
+ asinh(scalar_t(0)))>>));
EXPECT_NEAR(angle::radian_t(1.443635475178810).value(),
asinh(scalar_t(2)).value(), 5.0e-9);
EXPECT_NEAR(angle::degree_t(82.714219883108939).value(),
@@ -2876,9 +2876,9 @@
}
TEST_F(UnitMath, atanh) {
- EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
- typename std::decay<decltype(
- atanh(scalar_t(0)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<angle::radian_t>,
+ typename std::decay_t<decltype(
+ atanh(scalar_t(0)))>>));
EXPECT_NEAR(angle::radian_t(0.549306144334055).value(),
atanh(scalar_t(0.5)).value(), 5.0e-9);
EXPECT_NEAR(angle::degree_t(31.472923730945389).value(),
@@ -2934,38 +2934,38 @@
auto sq = pow<2>(value);
EXPECT_NEAR(100.0, sq(), 5.0e-2);
- isSame = std::is_same<decltype(sq), square_meter_t>::value;
+ isSame = std::is_same_v<decltype(sq), square_meter_t>;
EXPECT_TRUE(isSame);
auto cube = pow<3>(value);
EXPECT_NEAR(1000.0, cube(), 5.0e-2);
- isSame = std::is_same<decltype(cube), unit_t<cubed<meter>>>::value;
+ isSame = std::is_same_v<decltype(cube), unit_t<cubed<meter>>>;
EXPECT_TRUE(isSame);
auto fourth = pow<4>(value);
EXPECT_NEAR(10000.0, fourth(), 5.0e-2);
- isSame = std::is_same<
+ isSame = std::is_same_v<
decltype(fourth),
- unit_t<compound_unit<squared<meter>, squared<meter>>>>::value;
+ unit_t<compound_unit<squared<meter>, squared<meter>>>>;
EXPECT_TRUE(isSame);
}
TEST_F(UnitMath, sqrt) {
- EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
- typename std::decay<decltype(sqrt(
- square_meter_t(4.0)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<meter_t>,
+ typename std::decay_t<decltype(sqrt(
+ square_meter_t(4.0)))>>));
EXPECT_NEAR(meter_t(2.0).value(),
sqrt(square_meter_t(4.0)).value(), 5.0e-9);
- EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
- typename std::decay<decltype(
- sqrt(steradian_t(16.0)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<angle::radian_t>,
+ typename std::decay_t<decltype(
+ sqrt(steradian_t(16.0)))>>));
EXPECT_NEAR(angle::radian_t(4.0).value(),
sqrt(steradian_t(16.0)).value(), 5.0e-9);
- EXPECT_TRUE((std::is_convertible<typename std::decay<foot_t>::type,
- typename std::decay<decltype(sqrt(
- square_foot_t(10.0)))>::type>::value));
+ EXPECT_TRUE((std::is_convertible_v<typename std::decay_t<foot_t>,
+ typename std::decay_t<decltype(sqrt(
+ square_foot_t(10.0)))>>));
// for rational conversion (i.e. no integral root) let's check a bunch of
// different ways this could go wrong
@@ -2978,15 +2978,15 @@
}
TEST_F(UnitMath, hypot) {
- EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
- typename std::decay<decltype(hypot(
- meter_t(3.0), meter_t(4.0)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<meter_t>,
+ typename std::decay_t<decltype(hypot(
+ meter_t(3.0), meter_t(4.0)))>>));
EXPECT_NEAR(meter_t(5.0).value(),
(hypot(meter_t(3.0), meter_t(4.0))).value(), 5.0e-9);
- EXPECT_TRUE((std::is_same<typename std::decay<foot_t>::type,
- typename std::decay<decltype(hypot(
- foot_t(3.0), meter_t(1.2192)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<foot_t>,
+ typename std::decay_t<decltype(hypot(
+ foot_t(3.0), meter_t(1.2192)))>>));
EXPECT_NEAR(foot_t(5.0).value(),
(hypot(foot_t(3.0), meter_t(1.2192))).value(), 5.0e-9);
}
@@ -2994,9 +2994,9 @@
TEST_F(UnitMath, ceil) {
double val = 101.1;
EXPECT_EQ(std::ceil(val), ceil(meter_t(val)).value());
- EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
- typename std::decay<decltype(
- ceil(meter_t(val)))>::type>::value));
+ EXPECT_TRUE((std::is_same_v<typename std::decay_t<meter_t>,
+ typename std::decay_t<decltype(
+ ceil(meter_t(val)))>>));
}
TEST_F(UnitMath, floor) {
@@ -3158,32 +3158,32 @@
#endif
TEST_F(CompileTimeArithmetic, unit_value_t) {
- typedef unit_value_t<meters, 3, 2> mRatio;
+ using mRatio = unit_value_t<meters, 3, 2>;
EXPECT_EQ(meter_t(1.5), mRatio::value());
}
TEST_F(CompileTimeArithmetic, is_unit_value_t) {
using mRatio = unit_value_t<meters, 3, 2>;
- EXPECT_TRUE((traits::is_unit_value_t<mRatio>::value));
- EXPECT_FALSE((traits::is_unit_value_t<meter_t>::value));
- EXPECT_FALSE((traits::is_unit_value_t<double>::value));
+ EXPECT_TRUE((traits::is_unit_value_t_v<mRatio>));
+ EXPECT_FALSE((traits::is_unit_value_t_v<meter_t>));
+ EXPECT_FALSE((traits::is_unit_value_t_v<double>));
- EXPECT_TRUE((traits::is_unit_value_t<mRatio, meters>::value));
- EXPECT_FALSE((traits::is_unit_value_t<meter_t, meters>::value));
- EXPECT_FALSE((traits::is_unit_value_t<double, meters>::value));
+ EXPECT_TRUE((traits::is_unit_value_t_v<mRatio, meters>));
+ EXPECT_FALSE((traits::is_unit_value_t_v<meter_t, meters>));
+ EXPECT_FALSE((traits::is_unit_value_t_v<double, meters>));
}
TEST_F(CompileTimeArithmetic, is_unit_value_t_category) {
using mRatio = unit_value_t<feet, 3, 2>;
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::length_unit, mRatio>::value));
+ (traits::is_unit_value_t_category_v<category::length_unit, mRatio>));
EXPECT_FALSE(
- (traits::is_unit_value_t_category<category::angle_unit, mRatio>::value));
+ (traits::is_unit_value_t_category_v<category::angle_unit, mRatio>));
EXPECT_FALSE((
- traits::is_unit_value_t_category<category::length_unit, meter_t>::value));
+ traits::is_unit_value_t_category_v<category::length_unit, meter_t>));
EXPECT_FALSE(
- (traits::is_unit_value_t_category<category::length_unit, double>::value));
+ (traits::is_unit_value_t_category_v<category::length_unit, double>));
}
TEST_F(CompileTimeArithmetic, unit_value_add) {
@@ -3192,39 +3192,39 @@
using sum = unit_value_add<mRatio, mRatio>;
EXPECT_EQ(meter_t(3.0), sum::value());
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::length_unit, sum>::value));
+ (traits::is_unit_value_t_category_v<category::length_unit, sum>));
using ftRatio = unit_value_t<feet, 1>;
using sumf = unit_value_add<ftRatio, mRatio>;
EXPECT_TRUE((
- std::is_same<typename std::decay<foot_t>::type,
- typename std::decay<decltype(sumf::value())>::type>::value));
+ std::is_same_v<typename std::decay_t<foot_t>,
+ typename std::decay_t<decltype(sumf::value())>>));
EXPECT_NEAR(5.92125984, sumf::value().value(), 5.0e-8);
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::length_unit, sumf>::value));
+ (traits::is_unit_value_t_category_v<category::length_unit, sumf>));
using cRatio = unit_value_t<celsius, 1>;
using fRatio = unit_value_t<fahrenheit, 2>;
using sumc = unit_value_add<cRatio, fRatio>;
EXPECT_TRUE((
- std::is_same<typename std::decay<celsius_t>::type,
- typename std::decay<decltype(sumc::value())>::type>::value));
+ std::is_same_v<typename std::decay_t<celsius_t>,
+ typename std::decay_t<decltype(sumc::value())>>));
EXPECT_NEAR(2.11111111111, sumc::value().value(), 5.0e-8);
- EXPECT_TRUE((traits::is_unit_value_t_category<category::temperature_unit,
- sumc>::value));
+ EXPECT_TRUE((traits::is_unit_value_t_category_v<category::temperature_unit,
+ sumc>));
using rRatio = unit_value_t<angle::radian, 1>;
using dRatio = unit_value_t<angle::degree, 3>;
using sumr = unit_value_add<rRatio, dRatio>;
EXPECT_TRUE((
- std::is_same<typename std::decay<angle::radian_t>::type,
- typename std::decay<decltype(sumr::value())>::type>::value));
+ std::is_same_v<typename std::decay_t<angle::radian_t>,
+ typename std::decay_t<decltype(sumr::value())>>));
EXPECT_NEAR(1.05235988, sumr::value().value(), 5.0e-8);
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::angle_unit, sumr>::value));
+ (traits::is_unit_value_t_category_v<category::angle_unit, sumr>));
}
TEST_F(CompileTimeArithmetic, unit_value_subtract) {
@@ -3233,39 +3233,39 @@
using diff = unit_value_subtract<mRatio, mRatio>;
EXPECT_EQ(meter_t(0), diff::value());
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::length_unit, diff>::value));
+ (traits::is_unit_value_t_category_v<category::length_unit, diff>));
using ftRatio = unit_value_t<feet, 1>;
using difff = unit_value_subtract<ftRatio, mRatio>;
- EXPECT_TRUE((std::is_same<
- typename std::decay<foot_t>::type,
- typename std::decay<decltype(difff::value())>::type>::value));
+ EXPECT_TRUE((std::is_same_v<
+ typename std::decay_t<foot_t>,
+ typename std::decay_t<decltype(difff::value())>>));
EXPECT_NEAR(-3.92125984, difff::value().value(), 5.0e-8);
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::length_unit, difff>::value));
+ (traits::is_unit_value_t_category_v<category::length_unit, difff>));
using cRatio = unit_value_t<celsius, 1>;
using fRatio = unit_value_t<fahrenheit, 2>;
using diffc = unit_value_subtract<cRatio, fRatio>;
- EXPECT_TRUE((std::is_same<
- typename std::decay<celsius_t>::type,
- typename std::decay<decltype(diffc::value())>::type>::value));
+ EXPECT_TRUE((std::is_same_v<
+ typename std::decay_t<celsius_t>,
+ typename std::decay_t<decltype(diffc::value())>>));
EXPECT_NEAR(-0.11111111111, diffc::value().value(), 5.0e-8);
- EXPECT_TRUE((traits::is_unit_value_t_category<category::temperature_unit,
- diffc>::value));
+ EXPECT_TRUE((traits::is_unit_value_t_category_v<category::temperature_unit,
+ diffc>));
using rRatio = unit_value_t<angle::radian, 1>;
using dRatio = unit_value_t<angle::degree, 3>;
using diffr = unit_value_subtract<rRatio, dRatio>;
- EXPECT_TRUE((std::is_same<
- typename std::decay<angle::radian_t>::type,
- typename std::decay<decltype(diffr::value())>::type>::value));
+ EXPECT_TRUE((std::is_same_v<
+ typename std::decay_t<angle::radian_t>,
+ typename std::decay_t<decltype(diffr::value())>>));
EXPECT_NEAR(0.947640122, diffr::value().value(), 5.0e-8);
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::angle_unit, diffr>::value));
+ (traits::is_unit_value_t_category_v<category::angle_unit, diffr>));
}
TEST_F(CompileTimeArithmetic, unit_value_multiply) {
@@ -3275,63 +3275,63 @@
using product = unit_value_multiply<mRatio, mRatio>;
EXPECT_EQ(square_meter_t(4), product::value());
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::area_unit, product>::value));
+ (traits::is_unit_value_t_category_v<category::area_unit, product>));
using productM = unit_value_multiply<mRatio, ftRatio>;
- EXPECT_TRUE((std::is_same<
- typename std::decay<square_meter_t>::type,
- typename std::decay<decltype(productM::value())>::type>::value));
+ EXPECT_TRUE((std::is_same_v<
+ typename std::decay_t<square_meter_t>,
+ typename std::decay_t<decltype(productM::value())>>));
EXPECT_NEAR(4.0, productM::value().value(), 5.0e-7);
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::area_unit, productM>::value));
+ (traits::is_unit_value_t_category_v<category::area_unit, productM>));
using productF = unit_value_multiply<ftRatio, mRatio>;
- EXPECT_TRUE((std::is_same<
- typename std::decay<square_foot_t>::type,
- typename std::decay<decltype(productF::value())>::type>::value));
+ EXPECT_TRUE((std::is_same_v<
+ typename std::decay_t<square_foot_t>,
+ typename std::decay_t<decltype(productF::value())>>));
EXPECT_NEAR(43.0556444224, productF::value().value(), 5.0e-6);
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::area_unit, productF>::value));
+ (traits::is_unit_value_t_category_v<category::area_unit, productF>));
using productF2 = unit_value_multiply<ftRatio, ftRatio>;
EXPECT_TRUE(
- (std::is_same<
- typename std::decay<square_foot_t>::type,
- typename std::decay<decltype(productF2::value())>::type>::value));
+ (std::is_same_v<
+ typename std::decay_t<square_foot_t>,
+ typename std::decay_t<decltype(productF2::value())>>));
EXPECT_NEAR(43.0556444224, productF2::value().value(), 5.0e-8);
EXPECT_TRUE((
- traits::is_unit_value_t_category<category::area_unit, productF2>::value));
+ traits::is_unit_value_t_category_v<category::area_unit, productF2>));
using nRatio = unit_value_t<units::force::newton, 5>;
using productN = unit_value_multiply<nRatio, ftRatio>;
EXPECT_FALSE(
- (std::is_same<
- typename std::decay<torque::newton_meter_t>::type,
- typename std::decay<decltype(productN::value())>::type>::value));
- EXPECT_TRUE((std::is_convertible<
- typename std::decay<torque::newton_meter_t>::type,
- typename std::decay<decltype(productN::value())>::type>::value));
+ (std::is_same_v<
+ typename std::decay_t<torque::newton_meter_t>,
+ typename std::decay_t<decltype(productN::value())>>));
+ EXPECT_TRUE((std::is_convertible_v<
+ typename std::decay_t<torque::newton_meter_t>,
+ typename std::decay_t<decltype(productN::value())>>));
EXPECT_NEAR(32.8084, productN::value().value(), 5.0e-8);
EXPECT_NEAR(10.0, (productN::value().convert<newton_meter>().value()),
5.0e-7);
- EXPECT_TRUE((traits::is_unit_value_t_category<category::torque_unit,
- productN>::value));
+ EXPECT_TRUE((traits::is_unit_value_t_category_v<category::torque_unit,
+ productN>));
using r1Ratio = unit_value_t<angle::radian, 11, 10>;
using r2Ratio = unit_value_t<angle::radian, 22, 10>;
using productR = unit_value_multiply<r1Ratio, r2Ratio>;
- EXPECT_TRUE((std::is_same<
- typename std::decay<steradian_t>::type,
- typename std::decay<decltype(productR::value())>::type>::value));
+ EXPECT_TRUE((std::is_same_v<
+ typename std::decay_t<steradian_t>,
+ typename std::decay_t<decltype(productR::value())>>));
EXPECT_NEAR(2.42, productR::value().value(), 5.0e-8);
EXPECT_NEAR(7944.39137,
(productR::value().convert<degrees_squared>().value()),
5.0e-6);
- EXPECT_TRUE((traits::is_unit_value_t_category<category::solid_angle_unit,
- productR>::value));
+ EXPECT_TRUE((traits::is_unit_value_t_category_v<category::solid_angle_unit,
+ productR>));
}
TEST_F(CompileTimeArithmetic, unit_value_divide) {
@@ -3341,116 +3341,116 @@
using product = unit_value_divide<mRatio, mRatio>;
EXPECT_EQ(scalar_t(1), product::value());
EXPECT_TRUE((
- traits::is_unit_value_t_category<category::scalar_unit, product>::value));
+ traits::is_unit_value_t_category_v<category::scalar_unit, product>));
using productM = unit_value_divide<mRatio, ftRatio>;
- EXPECT_TRUE((std::is_same<
- typename std::decay<scalar_t>::type,
- typename std::decay<decltype(productM::value())>::type>::value));
+ EXPECT_TRUE((std::is_same_v<
+ typename std::decay_t<scalar_t>,
+ typename std::decay_t<decltype(productM::value())>>));
EXPECT_NEAR(1, productM::value().value(), 5.0e-7);
- EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
- productM>::value));
+ EXPECT_TRUE((traits::is_unit_value_t_category_v<category::scalar_unit,
+ productM>));
using productF = unit_value_divide<ftRatio, mRatio>;
- EXPECT_TRUE((std::is_same<
- typename std::decay<scalar_t>::type,
- typename std::decay<decltype(productF::value())>::type>::value));
+ EXPECT_TRUE((std::is_same_v<
+ typename std::decay_t<scalar_t>,
+ typename std::decay_t<decltype(productF::value())>>));
EXPECT_NEAR(1.0, productF::value().value(), 5.0e-6);
- EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
- productF>::value));
+ EXPECT_TRUE((traits::is_unit_value_t_category_v<category::scalar_unit,
+ productF>));
using productF2 = unit_value_divide<ftRatio, ftRatio>;
EXPECT_TRUE(
- (std::is_same<
- typename std::decay<scalar_t>::type,
- typename std::decay<decltype(productF2::value())>::type>::value));
+ (std::is_same_v<
+ typename std::decay_t<scalar_t>,
+ typename std::decay_t<decltype(productF2::value())>>));
EXPECT_NEAR(1.0, productF2::value().value(), 5.0e-8);
- EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
- productF2>::value));
+ EXPECT_TRUE((traits::is_unit_value_t_category_v<category::scalar_unit,
+ productF2>));
using sRatio = unit_value_t<seconds, 10>;
using productMS = unit_value_divide<mRatio, sRatio>;
EXPECT_TRUE(
- (std::is_same<
- typename std::decay<meters_per_second_t>::type,
- typename std::decay<decltype(productMS::value())>::type>::value));
+ (std::is_same_v<
+ typename std::decay_t<meters_per_second_t>,
+ typename std::decay_t<decltype(productMS::value())>>));
EXPECT_NEAR(0.2, productMS::value().value(), 5.0e-8);
- EXPECT_TRUE((traits::is_unit_value_t_category<category::velocity_unit,
- productMS>::value));
+ EXPECT_TRUE((traits::is_unit_value_t_category_v<category::velocity_unit,
+ productMS>));
using rRatio = unit_value_t<angle::radian, 20>;
using productRS = unit_value_divide<rRatio, sRatio>;
EXPECT_TRUE(
- (std::is_same<
- typename std::decay<radians_per_second_t>::type,
- typename std::decay<decltype(productRS::value())>::type>::value));
+ (std::is_same_v<
+ typename std::decay_t<radians_per_second_t>,
+ typename std::decay_t<decltype(productRS::value())>>));
EXPECT_NEAR(2, productRS::value().value(), 5.0e-8);
EXPECT_NEAR(114.592,
(productRS::value().convert<degrees_per_second>().value()),
5.0e-4);
- EXPECT_TRUE((traits::is_unit_value_t_category<category::angular_velocity_unit,
- productRS>::value));
+ EXPECT_TRUE((traits::is_unit_value_t_category_v<category::angular_velocity_unit,
+ productRS>));
}
TEST_F(CompileTimeArithmetic, unit_value_power) {
using mRatio = unit_value_t<meters, 2>;
using sq = unit_value_power<mRatio, 2>;
- EXPECT_TRUE((std::is_convertible<
- typename std::decay<square_meter_t>::type,
- typename std::decay<decltype(sq::value())>::type>::value));
+ EXPECT_TRUE((std::is_convertible_v<
+ typename std::decay_t<square_meter_t>,
+ typename std::decay_t<decltype(sq::value())>>));
EXPECT_NEAR(4, sq::value().value(), 5.0e-8);
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::area_unit, sq>::value));
+ (traits::is_unit_value_t_category_v<category::area_unit, sq>));
using rRatio = unit_value_t<angle::radian, 18, 10>;
using sqr = unit_value_power<rRatio, 2>;
- EXPECT_TRUE((std::is_convertible<
- typename std::decay<steradian_t>::type,
- typename std::decay<decltype(sqr::value())>::type>::value));
+ EXPECT_TRUE((std::is_convertible_v<
+ typename std::decay_t<steradian_t>,
+ typename std::decay_t<decltype(sqr::value())>>));
EXPECT_NEAR(3.24, sqr::value().value(), 5.0e-8);
EXPECT_NEAR(10636.292574038049895092690529904,
(sqr::value().convert<degrees_squared>().value()), 5.0e-10);
- EXPECT_TRUE((traits::is_unit_value_t_category<category::solid_angle_unit,
- sqr>::value));
+ EXPECT_TRUE((traits::is_unit_value_t_category_v<category::solid_angle_unit,
+ sqr>));
}
TEST_F(CompileTimeArithmetic, unit_value_sqrt) {
using mRatio = unit_value_t<square_meters, 10>;
using root = unit_value_sqrt<mRatio>;
- EXPECT_TRUE((std::is_convertible<
- typename std::decay<meter_t>::type,
- typename std::decay<decltype(root::value())>::type>::value));
+ EXPECT_TRUE((std::is_convertible_v<
+ typename std::decay_t<meter_t>,
+ typename std::decay_t<decltype(root::value())>>));
EXPECT_NEAR(3.16227766017, root::value().value(), 5.0e-9);
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::length_unit, root>::value));
+ (traits::is_unit_value_t_category_v<category::length_unit, root>));
using hRatio = unit_value_t<hectare, 51, 7>;
using rooth = unit_value_sqrt<hRatio, 100000000>;
- EXPECT_TRUE((std::is_convertible<
- typename std::decay<mile_t>::type,
- typename std::decay<decltype(rooth::value())>::type>::value));
+ EXPECT_TRUE((std::is_convertible_v<
+ typename std::decay_t<mile_t>,
+ typename std::decay_t<decltype(rooth::value())>>));
EXPECT_NEAR(2.69920623253, rooth::value().value(), 5.0e-8);
EXPECT_NEAR(269.920623, rooth::value().convert<meters>().value(),
5.0e-6);
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::length_unit, rooth>::value));
+ (traits::is_unit_value_t_category_v<category::length_unit, rooth>));
using rRatio = unit_value_t<steradian, 18, 10>;
using rootr = unit_value_sqrt<rRatio>;
- EXPECT_TRUE((traits::is_angle_unit<decltype(rootr::value())>::value));
+ EXPECT_TRUE((traits::is_angle_unit_v<decltype(rootr::value())>));
EXPECT_NEAR(1.3416407865, rootr::value().value(), 5.0e-8);
EXPECT_NEAR(76.870352574,
rootr::value().convert<angle::degrees>().value(), 5.0e-6);
EXPECT_TRUE(
- (traits::is_unit_value_t_category<category::angle_unit, rootr>::value));
+ (traits::is_unit_value_t_category_v<category::angle_unit, rootr>));
}
TEST_F(CaseStudies, radarRangeEquation) {
diff --git a/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp
new file mode 100644
index 0000000..4dfaa0e
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/BangBangController.h"
+#include "gtest/gtest.h"
+
+class BangBangInputOutputTest : public testing::Test {
+ protected:
+ frc::BangBangController controller;
+};
+
+TEST_F(BangBangInputOutputTest, ShouldOutput) {
+ EXPECT_DOUBLE_EQ(controller.Calculate(0, 1), 1);
+}
+
+TEST_F(BangBangInputOutputTest, ShouldNotOutput) {
+ EXPECT_DOUBLE_EQ(controller.Calculate(1, 0), 0);
+}
diff --git a/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp b/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp
new file mode 100644
index 0000000..e582720
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp
@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/BangBangController.h"
+#include "gtest/gtest.h"
+
+class BangBangToleranceTest : public testing::Test {
+ protected:
+ frc::BangBangController controller{0.1};
+};
+
+TEST_F(BangBangToleranceTest, InTolerance) {
+ controller.SetSetpoint(1);
+ controller.Calculate(1);
+ EXPECT_TRUE(controller.AtSetpoint());
+}
+
+TEST_F(BangBangToleranceTest, OutOfTolerance) {
+ controller.SetSetpoint(1);
+ controller.Calculate(0);
+ EXPECT_FALSE(controller.AtSetpoint());
+}
diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
index 8c52cd0..7055530 100644
--- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
@@ -30,11 +30,11 @@
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
}();
- LinearQuadraticRegulator<2, 1> controller{
- plant, {0.02, 0.4}, {12.0}, 0.00505_s};
+ Eigen::Matrix<double, 1, 2> K =
+ LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5.05_ms}.K();
- EXPECT_NEAR(522.15314269, controller.K(0, 0), 1e-6);
- EXPECT_NEAR(38.20138596, controller.K(0, 1), 1e-6);
+ EXPECT_NEAR(522.15314269, K(0, 0), 1e-6);
+ EXPECT_NEAR(38.20138596, K(0, 1), 1e-6);
}
TEST(LinearQuadraticRegulatorTest, ArmGains) {
@@ -54,11 +54,12 @@
motors, 1.0 / 3.0 * m * r * r, G);
}();
- LinearQuadraticRegulator<2, 1> controller{
- plant, {0.01745, 0.08726}, {12.0}, 0.00505_s};
+ Eigen::Matrix<double, 1, 2> K =
+ LinearQuadraticRegulator<2, 1>{plant, {0.01745, 0.08726}, {12.0}, 5.05_ms}
+ .K();
- EXPECT_NEAR(19.16, controller.K(0, 0), 1e-1);
- EXPECT_NEAR(3.32, controller.K(0, 1), 1e-1);
+ EXPECT_NEAR(19.16, K(0, 0), 1e-1);
+ EXPECT_NEAR(3.32, K(0, 1), 1e-1);
}
TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
@@ -76,10 +77,99 @@
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
}();
- LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.020_s};
+ Eigen::Matrix<double, 1, 2> K =
+ LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K();
- EXPECT_NEAR(10.38, controller.K(0, 0), 1e-1);
- EXPECT_NEAR(0.69, controller.K(0, 1), 1e-1);
+ EXPECT_NEAR(10.38, K(0, 0), 1e-1);
+ EXPECT_NEAR(0.69, K(0, 1), 1e-1);
+}
+
+/**
+ * Returns feedback control gain for implicit model following.
+ *
+ * This is used to test the QRN overload of LQR.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @param A State matrix.
+ * @param B Input matrix.
+ * @param Q State cost matrix.
+ * @param R Input cost matrix.
+ * @param Aref Desired state matrix.
+ * @param dt Discretization timestep.
+ */
+template <int States, int Inputs>
+Eigen::Matrix<double, Inputs, States> GetImplicitModelFollowingK(
+ const Eigen::Matrix<double, States, States>& A,
+ const Eigen::Matrix<double, States, Inputs>& B,
+ const Eigen::Matrix<double, States, States>& Q,
+ const Eigen::Matrix<double, Inputs, Inputs>& R,
+ const Eigen::Matrix<double, States, States>& Aref, units::second_t dt) {
+ // Discretize real dynamics
+ Eigen::Matrix<double, States, States> discA;
+ Eigen::Matrix<double, States, Inputs> discB;
+ DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+ // Discretize desired dynamics
+ Eigen::Matrix<double, States, States> discAref;
+ DiscretizeA<States>(Aref, dt, &discAref);
+
+ Eigen::Matrix<double, States, States> Qimf =
+ (discA - discAref).transpose() * Q * (discA - discAref);
+ Eigen::Matrix<double, Inputs, Inputs> Rimf =
+ discB.transpose() * Q * discB + R;
+ Eigen::Matrix<double, States, Inputs> Nimf =
+ (discA - discAref).transpose() * Q * discB;
+
+ return LinearQuadraticRegulator<States, Inputs>{A, B, Qimf, Rimf, Nimf, dt}
+ .K();
+}
+
+TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithSingleIntegrator) {
+ Eigen::Matrix<double, 2, 2> A{Eigen::Matrix<double, 2, 2>::Zero()};
+ Eigen::Matrix<double, 2, 2> B{Eigen::Matrix<double, 2, 2>::Identity()};
+ Eigen::Matrix<double, 2, 2> Q{Eigen::Matrix<double, 2, 2>::Identity()};
+ Eigen::Matrix<double, 2, 2> R{Eigen::Matrix<double, 2, 2>::Identity()};
+
+ // QR overload
+ Eigen::Matrix<double, 2, 2> K =
+ LinearQuadraticRegulator<2, 2>{A, B, Q, R, 5_ms}.K();
+ EXPECT_NEAR(0.99750312499512261, K(0, 0), 1e-10);
+ EXPECT_NEAR(0.0, K(0, 1), 1e-10);
+ EXPECT_NEAR(0.0, K(1, 0), 1e-10);
+ EXPECT_NEAR(0.99750312499512261, K(1, 1), 1e-10);
+
+ // QRN overload
+ Eigen::Matrix<double, 2, 2> N{Eigen::Matrix<double, 2, 2>::Identity()};
+ Eigen::Matrix<double, 2, 2> Kimf =
+ LinearQuadraticRegulator<2, 2>{A, B, Q, R, N, 5_ms}.K();
+ EXPECT_NEAR(1.0, Kimf(0, 0), 1e-10);
+ EXPECT_NEAR(0.0, Kimf(0, 1), 1e-10);
+ EXPECT_NEAR(0.0, Kimf(1, 0), 1e-10);
+ EXPECT_NEAR(1.0, Kimf(1, 1), 1e-10);
+}
+
+TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithDoubleIntegrator) {
+ double Kv = 3.02;
+ double Ka = 0.642;
+
+ Eigen::Matrix<double, 2, 2> A{{0, 1}, {0, -Kv / Ka}};
+ Eigen::Matrix<double, 2, 1> B{{0}, {1.0 / Ka}};
+ Eigen::Matrix<double, 2, 2> Q{{1, 0}, {0, 0.2}};
+ Eigen::Matrix<double, 1, 1> R{0.25};
+
+ // QR overload
+ Eigen::Matrix<double, 1, 2> K =
+ LinearQuadraticRegulator<2, 1>{A, B, Q, R, 5_ms}.K();
+ EXPECT_NEAR(1.9960017786537287, K(0, 0), 1e-10);
+ EXPECT_NEAR(0.51182128351092726, K(0, 1), 1e-10);
+
+ // QRN overload
+ Eigen::Matrix<double, 2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}};
+ Eigen::Matrix<double, 1, 2> Kimf =
+ GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms);
+ EXPECT_NEAR(0.0, Kimf(0, 0), 1e-10);
+ EXPECT_NEAR(-5.367540084534802e-05, Kimf(0, 1), 1e-10);
}
TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
diff --git a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
index 5e297f4..8829deb 100644
--- a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
@@ -16,7 +16,8 @@
180.0};
TEST(RamseteControllerTest, ReachesReference) {
- frc::RamseteController controller{2.0, 0.7};
+ frc::RamseteController controller{2.0 * 1_rad * 1_rad / (1_m * 1_m),
+ 0.7 / 1_rad};
frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
diff --git a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
new file mode 100644
index 0000000..2f64908
--- /dev/null
+++ b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
@@ -0,0 +1,58 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <wpi/timestamp.h>
+
+#include "frc/filter/Debouncer.h"
+#include "gtest/gtest.h"
+#include "units/time.h"
+
+static units::second_t now = 0_s;
+
+class DebouncerTest : public ::testing::Test {
+ protected:
+ void SetUp() override {
+ WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
+ }
+
+ void TearDown() override { WPI_SetNowImpl(nullptr); }
+};
+
+TEST_F(DebouncerTest, DebounceRising) {
+ frc::Debouncer debouncer{20_ms};
+
+ debouncer.Calculate(false);
+ EXPECT_FALSE(debouncer.Calculate(true));
+
+ now += 1_s;
+
+ EXPECT_TRUE(debouncer.Calculate(true));
+}
+
+TEST_F(DebouncerTest, DebounceFalling) {
+ frc::Debouncer debouncer{20_ms, frc::Debouncer::DebounceType::kFalling};
+
+ debouncer.Calculate(true);
+ EXPECT_TRUE(debouncer.Calculate(false));
+
+ now += 1_s;
+
+ EXPECT_FALSE(debouncer.Calculate(false));
+}
+
+TEST_F(DebouncerTest, DebounceBoth) {
+ frc::Debouncer debouncer{20_ms, frc::Debouncer::DebounceType::kBoth};
+
+ debouncer.Calculate(false);
+ EXPECT_FALSE(debouncer.Calculate(true));
+
+ now += 1_s;
+
+ EXPECT_TRUE(debouncer.Calculate(true));
+ EXPECT_TRUE(debouncer.Calculate(false));
+
+ now += 1_s;
+
+ EXPECT_FALSE(debouncer.Calculate(false));
+}
diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
index bca3f9d..56121fa 100644
--- a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
@@ -9,6 +9,7 @@
#include <memory>
#include <random>
+#include <wpi/array.h>
#include <wpi/numbers>
#include "gtest/gtest.h"
@@ -120,8 +121,40 @@
kTestMovAvg, kTestPulse));
template <int Derivative, int Samples, typename F, typename DfDx>
-void AssertResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
- double max) {
+void AssertCentralResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
+ double max) {
+ static_assert(Samples % 2 != 0, "Number of samples must be odd.");
+
+ // Generate stencil points from -(samples - 1)/2 to (samples - 1)/2
+ wpi::array<int, Samples> stencil{wpi::empty_array};
+ for (int i = 0; i < Samples; ++i) {
+ stencil[i] = -(Samples - 1) / 2 + i;
+ }
+
+ auto filter =
+ frc::LinearFilter<double>::FiniteDifference<Derivative, Samples>(stencil,
+ h);
+
+ for (int i = min / h.value(); i < max / h.value(); ++i) {
+ // Let filter initialize
+ if (i < static_cast<int>(min / h.value()) + Samples) {
+ filter.Calculate(f(i * h.value()));
+ continue;
+ }
+
+ // For central finite difference, the derivative computed at this point is
+ // half the window size in the past.
+ // The order of accuracy is O(h^(N - d)) where N is number of stencil
+ // points and d is order of derivative
+ EXPECT_NEAR(dfdx((i - static_cast<int>((Samples - 1) / 2)) * h.value()),
+ filter.Calculate(f(i * h.value())),
+ std::pow(h.value(), Samples - Derivative));
+ }
+}
+
+template <int Derivative, int Samples, typename F, typename DfDx>
+void AssertBackwardResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
+ double max) {
auto filter =
frc::LinearFilter<double>::BackwardFiniteDifference<Derivative, Samples>(
h);
@@ -141,12 +174,12 @@
}
/**
- * Test backward finite difference.
+ * Test central finite difference.
*/
-TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
+TEST(LinearFilterOutputTest, CentralFiniteDifference) {
constexpr auto h = 5_ms;
- AssertResults<1, 2>(
+ AssertCentralResults<1, 3>(
[](double x) {
// f(x) = x²
return x * x;
@@ -157,7 +190,7 @@
},
h, -20.0, 20.0);
- AssertResults<1, 2>(
+ AssertCentralResults<1, 3>(
[](double x) {
// f(x) = std::sin(x)
return std::sin(x);
@@ -168,7 +201,7 @@
},
h, -20.0, 20.0);
- AssertResults<1, 2>(
+ AssertCentralResults<1, 3>(
[](double x) {
// f(x) = ln(x)
return std::log(x);
@@ -179,7 +212,7 @@
},
h, 1.0, 20.0);
- AssertResults<2, 4>(
+ AssertCentralResults<2, 5>(
[](double x) {
// f(x) = x^2
return x * x;
@@ -190,7 +223,7 @@
},
h, -20.0, 20.0);
- AssertResults<2, 4>(
+ AssertCentralResults<2, 5>(
[](double x) {
// f(x) = std::sin(x)
return std::sin(x);
@@ -201,7 +234,80 @@
},
h, -20.0, 20.0);
- AssertResults<2, 4>(
+ AssertCentralResults<2, 5>(
+ [](double x) {
+ // f(x) = ln(x)
+ return std::log(x);
+ },
+ [](double x) {
+ // d²f/dx² = -1 / x²
+ return -1.0 / (x * x);
+ },
+ h, 1.0, 20.0);
+}
+
+/**
+ * Test backward finite difference.
+ */
+TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
+ constexpr auto h = 5_ms;
+
+ AssertBackwardResults<1, 2>(
+ [](double x) {
+ // f(x) = x²
+ return x * x;
+ },
+ [](double x) {
+ // df/dx = 2x
+ return 2.0 * x;
+ },
+ h, -20.0, 20.0);
+
+ AssertBackwardResults<1, 2>(
+ [](double x) {
+ // f(x) = std::sin(x)
+ return std::sin(x);
+ },
+ [](double x) {
+ // df/dx = std::cos(x)
+ return std::cos(x);
+ },
+ h, -20.0, 20.0);
+
+ AssertBackwardResults<1, 2>(
+ [](double x) {
+ // f(x) = ln(x)
+ return std::log(x);
+ },
+ [](double x) {
+ // df/dx = 1 / x
+ return 1.0 / x;
+ },
+ h, 1.0, 20.0);
+
+ AssertBackwardResults<2, 4>(
+ [](double x) {
+ // f(x) = x^2
+ return x * x;
+ },
+ [](double x) {
+ // d²f/dx² = 2
+ return 2.0;
+ },
+ h, -20.0, 20.0);
+
+ AssertBackwardResults<2, 4>(
+ [](double x) {
+ // f(x) = std::sin(x)
+ return std::sin(x);
+ },
+ [](double x) {
+ // d²f/dx² = -std::sin(x)
+ return -std::sin(x);
+ },
+ h, -20.0, 20.0);
+
+ AssertBackwardResults<2, 4>(
[](double x) {
// f(x) = ln(x)
return std::log(x);
diff --git a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
index d2c0bae..4896073 100644
--- a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
@@ -12,7 +12,16 @@
static units::second_t now = 0_s;
-TEST(SlewRateLimiterTest, SlewRateLimit) {
+class SlewRateLimiterTest : public ::testing::Test {
+ protected:
+ void SetUp() override {
+ WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
+ }
+
+ void TearDown() override { WPI_SetNowImpl(nullptr); }
+};
+
+TEST_F(SlewRateLimiterTest, SlewRateLimit) {
WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
frc::SlewRateLimiter<units::meters> limiter(1_mps);
@@ -22,9 +31,7 @@
EXPECT_LT(limiter.Calculate(2_m), 2_m);
}
-TEST(SlewRateLimiterTest, SlewRateNoLimit) {
- WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
-
+TEST_F(SlewRateLimiterTest, SlewRateNoLimit) {
frc::SlewRateLimiter<units::meters> limiter(1_mps);
now += 1_s;
diff --git a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
new file mode 100644
index 0000000..ee2ec9b
--- /dev/null
+++ b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cmath>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
+#include "gtest/gtest.h"
+#include "units/time.h"
+
+TEST(TimeInterpolatableBufferTest, Interpolation) {
+ frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
+
+ buffer.AddSample(0_s, frc::Rotation2d(0_rad));
+ EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(0_rad));
+ buffer.AddSample(1_s, frc::Rotation2d(1_rad));
+ EXPECT_TRUE(buffer.Sample(0.5_s) == frc::Rotation2d(0.5_rad));
+ EXPECT_TRUE(buffer.Sample(1_s) == frc::Rotation2d(1_rad));
+ buffer.AddSample(3_s, frc::Rotation2d(2_rad));
+ EXPECT_TRUE(buffer.Sample(2_s) == frc::Rotation2d(1.5_rad));
+
+ buffer.AddSample(10.5_s, frc::Rotation2d(2_rad));
+ EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(1_rad));
+}
+
+TEST(TimeInterpolatableBufferTest, Pose2d) {
+ frc::TimeInterpolatableBuffer<frc::Pose2d> buffer{10_s};
+ // We expect to be at (1 - 1/std::sqrt(2), 1/std::sqrt(2), 45deg) at t=0.5
+ buffer.AddSample(0_s, frc::Pose2d{0_m, 0_m, 90_deg});
+ buffer.AddSample(1_s, frc::Pose2d{1_m, 1_m, 0_deg});
+ frc::Pose2d sample = buffer.Sample(0.5_s);
+ EXPECT_TRUE(std::abs(sample.X().to<double>() - (1 - 1 / std::sqrt(2))) <
+ 0.01);
+ EXPECT_TRUE(std::abs(sample.Y().to<double>() - (1 / std::sqrt(2))) < 0.01);
+ EXPECT_TRUE(std::abs(sample.Rotation().Degrees().to<double>() - 45) < 0.01);
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
index 18b72de..8cc454e 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
@@ -143,9 +143,9 @@
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
-TEST_F(MecanumDriveKinematicsTest, Normalize) {
+TEST_F(MecanumDriveKinematicsTest, Desaturate) {
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
- wheelSpeeds.Normalize(5.5_mps);
+ wheelSpeeds.Desaturate(5.5_mps);
double kFactor = 5.5 / 7.0;
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
index 9384d89..55ebdcf 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
@@ -162,14 +162,14 @@
EXPECT_NEAR(chassisSpeeds.omega.value(), 1.5, kEpsilon);
}
-TEST_F(SwerveDriveKinematicsTest, Normalize) {
+TEST_F(SwerveDriveKinematicsTest, Desaturate) {
SwerveModuleState state1{5.0_mps, Rotation2d()};
SwerveModuleState state2{6.0_mps, Rotation2d()};
SwerveModuleState state3{4.0_mps, Rotation2d()};
SwerveModuleState state4{7.0_mps, Rotation2d()};
wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
- SwerveDriveKinematics<4>::NormalizeWheelSpeeds(&arr, 5.5_mps);
+ SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 5.5_mps);
double kFactor = 5.5 / 7.0;
diff --git a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
index 69e202f..41c34f9 100644
--- a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -3,7 +3,6 @@
// the WPILib BSD license file in the root directory of this project.
#include <chrono>
-#include <iostream>
#include <vector>
#include "frc/geometry/Pose2d.h"
diff --git a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
index 25449fb..e045622 100644
--- a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
@@ -3,7 +3,6 @@
// the WPILib BSD license file in the root directory of this project.
#include <chrono>
-#include <iostream>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
index 1fa12c2..fbf86f1 100644
--- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
+++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
@@ -49,6 +49,18 @@
ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
}
+TEST(LinearSystemIDTest, DCMotorSystem) {
+ auto model = frc::LinearSystemId::DCMotorSystem(frc::DCMotor::NEO(2),
+ 0.00032_kg_sq_m, 1.0);
+ ASSERT_TRUE(model.A().isApprox(
+ Eigen::Matrix<double, 2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
+ ASSERT_TRUE(
+ model.B().isApprox(Eigen::Matrix<double, 2, 1>{0, 1354.166667}, 0.001));
+ ASSERT_TRUE(model.C().isApprox(
+ Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
+ ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 0.0}, 0.001));
+}
+
TEST(LinearSystemIDTest, IdentifyPositionSystem) {
// By controls engineering in frc,
// x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
index b21ef7b..b5f6406 100644
--- a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
@@ -2,7 +2,6 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
-#include <iostream>
#include <memory>
#include <vector>